diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index e0209d02..5899ff49 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -17,14 +17,14 @@ jobs: fail-fast: false matrix: ros_distribution: - - humble + # - humble - jazzy - rolling include: # ROS 2 Humble Hawksbill - - docker_image: ubuntu:jammy - ros_distribution: humble - ros_version: 2 + # - docker_image: ubuntu:jammy + # ros_distribution: humble + # ros_version: 2 # ROS 2 Jazzy Jalisco - docker_image: ubuntu:noble ros_distribution: jazzy diff --git a/turtlebot3_fake_node/CHANGELOG.rst b/turtlebot3_fake_node/CHANGELOG.rst index 12775136..0ce85ea1 100644 --- a/turtlebot3_fake_node/CHANGELOG.rst +++ b/turtlebot3_fake_node/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package turtlebot3_fake ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.5 (2025-06-04) +------------------ +* None + 2.3.3 (2025-05-29) ------------------ * Deprecate ament_include_dependency usage in CMakeLists.txt, related PR(https://github.com/ROBOTIS-GIT/turtlebot3_simulations/pull/234) diff --git a/turtlebot3_fake_node/package.xml b/turtlebot3_fake_node/package.xml index 1334065a..9af9b2ec 100644 --- a/turtlebot3_fake_node/package.xml +++ b/turtlebot3_fake_node/package.xml @@ -2,7 +2,7 @@ turtlebot3_fake_node - 2.3.3 + 2.3.5 Package for TurtleBot3 fake node. With this package, simple tests can be done without a robot. You can do simple tests using this package on rviz without real robots. diff --git a/turtlebot3_gazebo/CHANGELOG.rst b/turtlebot3_gazebo/CHANGELOG.rst index 5f59fc07..3176ba28 100644 --- a/turtlebot3_gazebo/CHANGELOG.rst +++ b/turtlebot3_gazebo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package turtlebot3_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.5 (2025-06-04) +------------------ +* Added turtlebot3_machine_learning world and plugin +* Contributors: ChanHyeong Lee + 2.3.3 (2025-05-29) ------------------ * Deprecate ament_include_dependency usage in CMakeLists.txt, related PR(https://github.com/ROBOTIS-GIT/turtlebot3_simulations/pull/234) diff --git a/turtlebot3_gazebo/CMakeLists.txt b/turtlebot3_gazebo/CMakeLists.txt index a9516b10..bbbdf2ba 100644 --- a/turtlebot3_gazebo/CMakeLists.txt +++ b/turtlebot3_gazebo/CMakeLists.txt @@ -1,7 +1,7 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(turtlebot3_gazebo) if(NOT CMAKE_CXX_STANDARD) @@ -26,17 +26,26 @@ find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(tf2 REQUIRED) +find_package(gz_math_vendor REQUIRED) +find_package(gz-math REQUIRED) +find_package(gz_sim_vendor REQUIRED) +find_package(gz-sim REQUIRED) +find_package(gz_plugin_vendor REQUIRED) +find_package(gz-plugin REQUIRED) ################################################################################ # Build ################################################################################ link_directories( ${GAZEBO_LIBRARY_DIRS} + ${GZ_SIM_LIBRARY_DIRS} ) include_directories( include ${GAZEBO_INCLUDE_DIRS} + ${gz_sim_vendor_INCLUDE_DIRS} + ${GZ_SIM_INCLUDE_DIRS} ) set(EXEC_NAME "turtlebot3_drive") @@ -50,25 +59,29 @@ target_link_libraries(${EXEC_NAME} tf2::tf2 ) -# add_library(traffic_light_plugin SHARED src/traffic_light_plugin.cpp) -# target_link_libraries(traffic_light_plugin ${GAZEBO_LIBRARIES}) - -# add_library(traffic_bar_plugin SHARED src/traffic_bar_plugin.cpp) -# target_link_libraries(traffic_bar_plugin ${GAZEBO_LIBRARIES}) - -# add_library(obstacle1 SHARED src/obstacle1.cpp) -# target_link_libraries(obstacle1 ${GAZEBO_LIBRARIES}) - -# add_library(obstacle2 SHARED src/obstacle2.cpp) -# target_link_libraries(obstacle2 ${GAZEBO_LIBRARIES}) +set(OBSTACLE_LIBS + obstacles + obstacle1 + obstacle2 +) -# add_library(obstacles SHARED src/obstacles.cpp) -# target_link_libraries(obstacles ${GAZEBO_LIBRARIES}) +foreach(lib ${OBSTACLE_LIBS}) + add_library(${lib} SHARED src/${lib}.cpp) + target_link_libraries(${lib} + PUBLIC + gz-sim::gz-sim + gz-math::gz-math + ) +endforeach() ################################################################################ # Install ################################################################################ -install(TARGETS ${EXEC_NAME} +install(TARGETS + ${EXEC_NAME} + obstacles + obstacle1 + obstacle2 DESTINATION lib/${PROJECT_NAME} ) diff --git a/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle1.hpp b/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle1.hpp index 53770379..e3eade91 100644 --- a/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle1.hpp +++ b/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle1.hpp @@ -12,27 +12,51 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Ryan Shim +// Author: Ryan Shim, ChanHyeong Lee #ifndef TURTLEBOT3_GAZEBO__OBSTACLE1_HPP_ #define TURTLEBOT3_GAZEBO__OBSTACLE1_HPP_ -#include -#include -#include -#include +#include +#include +#include -namespace gazebo +#include +#include +#include + +namespace turtlebot3_gazebo { -class Obstacle1 : public ModelPlugin + +class Obstacle1Plugin + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: - void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) override; + Obstacle1Plugin() = default; + ~Obstacle1Plugin() override = default; + + void Configure( + const gz::sim::Entity & entity, + const std::shared_ptr & sdf, + gz::sim::EntityComponentManager & ecm, + gz::sim::EventManager & eventMgr) override; + + void PreUpdate( + const gz::sim::UpdateInfo & info, + gz::sim::EntityComponentManager & ecm) override; private: - physics::ModelPtr model; - event::ConnectionPtr updateConnection; + gz::sim::Model model; + std::chrono::steady_clock::time_point startTime; + + std::vector waypoints; + std::vector segmentDistances; + double totalDistance = 0.0; + double speed = 0.1; // meters per second }; -GZ_REGISTER_MODEL_PLUGIN(Obstacle1); -} // namespace gazebo + +} // namespace turtlebot3_gazebo + #endif // TURTLEBOT3_GAZEBO__OBSTACLE1_HPP_ diff --git a/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle2.hpp b/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle2.hpp index f99808d8..c85bf787 100644 --- a/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle2.hpp +++ b/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacle2.hpp @@ -12,28 +12,51 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Ryan Shim +// Author: Ryan Shim, ChanHyeong Lee #ifndef TURTLEBOT3_GAZEBO__OBSTACLE2_HPP_ #define TURTLEBOT3_GAZEBO__OBSTACLE2_HPP_ -#include -#include -#include -#include +#include +#include +#include -namespace gazebo +#include +#include +#include + +namespace turtlebot3_gazebo { -class Obstacle2 : public ModelPlugin + +class Obstacle2Plugin + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: - Obstacle2() = default; - void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) override; + Obstacle2Plugin() = default; + ~Obstacle2Plugin() override = default; + + void Configure( + const gz::sim::Entity & entity, + const std::shared_ptr & sdf, + gz::sim::EntityComponentManager & ecm, + gz::sim::EventManager & eventMgr) override; + + void PreUpdate( + const gz::sim::UpdateInfo & info, + gz::sim::EntityComponentManager & ecm) override; private: - physics::ModelPtr model; - event::ConnectionPtr updateConnection; + gz::sim::Model model; + std::chrono::steady_clock::time_point startTime; + + std::vector waypoints; + std::vector segmentDistances; + double totalDistance = 0.0; + double speed = 0.1; // meters per second }; -GZ_REGISTER_MODEL_PLUGIN(Obstacle2); -} // namespace gazebo + +} // namespace turtlebot3_gazebo + #endif // TURTLEBOT3_GAZEBO__OBSTACLE2_HPP_ diff --git a/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacles.hpp b/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacles.hpp index 5d88a9b4..341a930b 100644 --- a/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacles.hpp +++ b/turtlebot3_gazebo/include/turtlebot3_gazebo/obstacles.hpp @@ -12,30 +12,44 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Ryan Shim +// Author: Ryan Shim, ChanHyeong Lee #ifndef TURTLEBOT3_GAZEBO__OBSTACLES_HPP_ #define TURTLEBOT3_GAZEBO__OBSTACLES_HPP_ -#include -#include -#include -#include +#include +#include -#define PI 3.141592 +#include +#include +#include +#include +#include -namespace gazebo +namespace turtlebot3_gazebo { -class Obstacles : public ModelPlugin + +class ObstaclesPlugin + : public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate { public: - Obstacles() = default; - void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) override; + void Configure( + const gz::sim::Entity & entity, + const std::shared_ptr & sdf, + gz::sim::EntityComponentManager & ecm, + gz::sim::EventManager & eventMgr) override; + + void PreUpdate( + const gz::sim::UpdateInfo & info, + gz::sim::EntityComponentManager & ecm) override; private: - physics::ModelPtr model; - event::ConnectionPtr updateConnection; + gz::sim::Model model{gz::sim::kNullEntity}; + std::chrono::steady_clock::time_point startTime; }; -GZ_REGISTER_MODEL_PLUGIN(Obstacles); -} // namespace gazebo + +} // namespace turtlebot3_gazebo + #endif // TURTLEBOT3_GAZEBO__OBSTACLES_HPP_ diff --git a/turtlebot3_gazebo/launch/turtlebot3_dqn_stage1.launch.py b/turtlebot3_gazebo/launch/turtlebot3_dqn_stage1.launch.py new file mode 100644 index 00000000..048b82a4 --- /dev/null +++ b/turtlebot3_gazebo/launch/turtlebot3_dqn_stage1.launch.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +# +# 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. +# +# Authors: Joep Tool, ChanHyeong Lee + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import AppendEnvironmentVariable +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') + ros_gz_sim = get_package_share_directory('ros_gz_sim') + + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + x_pose = LaunchConfiguration('x_pose', default='0.0') + y_pose = LaunchConfiguration('y_pose', default='0.0') + + world = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'worlds', + 'turtlebot3_dqn_stage1.world' + ) + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'models' + ) + ) + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s -v4 ', world]}.items() + ) + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': '-g -v4 '}.items() + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': use_sim_time}.items() + ) + + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': x_pose, + 'y_pose': y_pose + }.items() + ) + + ld = LaunchDescription() + + ld.add_action(set_env_vars_resources) + ld.add_action(gzserver_cmd) + ld.add_action(gzclient_cmd) + ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_turtlebot_cmd) + + return ld diff --git a/turtlebot3_gazebo/launch/turtlebot3_dqn_stage2.launch.py b/turtlebot3_gazebo/launch/turtlebot3_dqn_stage2.launch.py new file mode 100644 index 00000000..6cfe0fb6 --- /dev/null +++ b/turtlebot3_gazebo/launch/turtlebot3_dqn_stage2.launch.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +# +# 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. +# +# Authors: Joep Tool, ChanHyeong Lee + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import AppendEnvironmentVariable +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') + ros_gz_sim = get_package_share_directory('ros_gz_sim') + + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + x_pose = LaunchConfiguration('x_pose', default='0.0') + y_pose = LaunchConfiguration('y_pose', default='0.0') + + world = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'worlds', + 'turtlebot3_dqn_stage2.world' + ) + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'models' + ) + ) + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s -v4 ', world]}.items() + ) + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': '-g -v4 '}.items() + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': use_sim_time}.items() + ) + + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': x_pose, + 'y_pose': y_pose + }.items() + ) + + ld = LaunchDescription() + + # Add the commands to the launch description + ld.add_action(set_env_vars_resources) + ld.add_action(gzserver_cmd) + ld.add_action(gzclient_cmd) + ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_turtlebot_cmd) + + return ld diff --git a/turtlebot3_gazebo/launch/turtlebot3_dqn_stage3.launch.py b/turtlebot3_gazebo/launch/turtlebot3_dqn_stage3.launch.py new file mode 100644 index 00000000..fa5dcc2c --- /dev/null +++ b/turtlebot3_gazebo/launch/turtlebot3_dqn_stage3.launch.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +# +# 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. +# +# Authors: Joep Tool, ChanHyeong Lee + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import AppendEnvironmentVariable +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') + ros_gz_sim = get_package_share_directory('ros_gz_sim') + + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + x_pose = LaunchConfiguration('x_pose', default='0.0') + y_pose = LaunchConfiguration('y_pose', default='0.0') + + world = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'worlds', + 'turtlebot3_dqn_stage3.world' + ) + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'models' + ) + ) + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s -v4 ', world]}.items() + ) + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': '-g -v4 '}.items() + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': use_sim_time}.items() + ) + + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': x_pose, + 'y_pose': y_pose + }.items() + ) + + ld = LaunchDescription() + + # Add the commands to the launch description + ld.add_action(set_env_vars_resources) + ld.add_action(gzserver_cmd) + ld.add_action(gzclient_cmd) + ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_turtlebot_cmd) + + return ld diff --git a/turtlebot3_gazebo/launch/turtlebot3_dqn_stage4.launch.py b/turtlebot3_gazebo/launch/turtlebot3_dqn_stage4.launch.py new file mode 100644 index 00000000..b539a123 --- /dev/null +++ b/turtlebot3_gazebo/launch/turtlebot3_dqn_stage4.launch.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +# +# 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. +# +# Authors: Joep Tool, ChanHyeong Lee + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import AppendEnvironmentVariable +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') + ros_gz_sim = get_package_share_directory('ros_gz_sim') + + use_sim_time = LaunchConfiguration('use_sim_time', default='true') + x_pose = LaunchConfiguration('x_pose', default='0.0') + y_pose = LaunchConfiguration('y_pose', default='0.0') + + world = os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'worlds', + 'turtlebot3_dqn_stage4.world' + ) + set_env_vars_resources = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join( + get_package_share_directory('turtlebot3_gazebo'), + 'models' + ) + ) + + gzserver_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': ['-r -s -v4 ', world]}.items() + ) + gzclient_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py') + ), + launch_arguments={'gz_args': '-g -v4 '}.items() + ) + + robot_state_publisher_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') + ), + launch_arguments={'use_sim_time': use_sim_time}.items() + ) + + spawn_turtlebot_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py') + ), + launch_arguments={ + 'x_pose': x_pose, + 'y_pose': y_pose + }.items() + ) + + ld = LaunchDescription() + + # Add the commands to the launch description + ld.add_action(set_env_vars_resources) + ld.add_action(gzserver_cmd) + ld.add_action(gzclient_cmd) + ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_turtlebot_cmd) + + return ld diff --git a/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf b/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf index 2a1ea27f..031697dc 100644 --- a/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf +++ b/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf @@ -386,14 +386,14 @@ 1.0 - cmd_vel + cmd_vel - odom - odom - base_footprint - 30 + odom + odom + base_footprint + 30 - /tf + /tf diff --git a/turtlebot3_gazebo/models/turtlebot3_burger_cam/model.config b/turtlebot3_gazebo/models/turtlebot3_burger_cam/model.config index 06a790f6..ae5f8e3a 100644 --- a/turtlebot3_gazebo/models/turtlebot3_burger_cam/model.config +++ b/turtlebot3_gazebo/models/turtlebot3_burger_cam/model.config @@ -4,7 +4,7 @@ TurtleBot3(Burger_cam) 2.0 model-1_4.sdf - model.sdf + model.sdf Taehun Lim(Darby) diff --git a/turtlebot3_gazebo/models/turtlebot3_burger_cam/model.sdf b/turtlebot3_gazebo/models/turtlebot3_burger_cam/model.sdf index d64e92b1..0797e402 100644 --- a/turtlebot3_gazebo/models/turtlebot3_burger_cam/model.sdf +++ b/turtlebot3_gazebo/models/turtlebot3_burger_cam/model.sdf @@ -1,5 +1,5 @@ - + 0.0 0.0 0.0 0.0 0.0 0.0 @@ -446,14 +446,14 @@ 1.0 - cmd_vel + cmd_vel - odom - odom - base_footprint - 30 + odom + odom + base_footprint + 30 - /tf + /tf diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/goal_box/model.config b/turtlebot3_gazebo/models/turtlebot3_dqn_world/goal_box/model.config new file mode 100644 index 00000000..e667c6a0 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/goal_box/model.config @@ -0,0 +1,11 @@ + + + goal_box + 1.0 + model.sdf + + + + + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/goal_box/model.sdf b/turtlebot3_gazebo/models/turtlebot3_dqn_world/goal_box/model.sdf new file mode 100644 index 00000000..6c55bb35 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/goal_box/model.sdf @@ -0,0 +1,24 @@ + + + + true + 0 0 0.0005 0 0 0 + + 0 0 0 0 0 0 + + + + 0.3 + 0.001 + + + + 1 0 0 1 + 1 0 0 1 + 0.1 0.1 0.1 1 + 0 0 0 1 + + + + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/model.config b/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/model.config new file mode 100644 index 00000000..6b17cee4 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/model.config @@ -0,0 +1,15 @@ + + + inner_walls + 1.0 + model.sdf + + + Ryan Shim + jhshim@robotis.com + + + + TurtleBot3 DQN World Inner Walls + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/model.sdf b/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/model.sdf new file mode 100644 index 00000000..2478e980 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/model.sdf @@ -0,0 +1,210 @@ + + + + 0 0 0 0 0 0 + true + + -2.0 -1.5 0.25 0 0 0 + + + + 1 0.15 0.5 + + + + + + + 1 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + -0.5 -2.0 0.25 0 0 -1.5708 + + + + 1 0.15 0.5 + + + + + + + 1 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + 1.0 -1.0 0.25 0 0 1.5708 + + + + 1 0.15 0.5 + + + + + + + 1 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + 1.2 1.9 0.25 0 0 -1.5708 + + + + 1 0.15 0.5 + + + + + + + 1 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + 1.9 0.4 0.25 0 0 0 + + + + 1 0.15 0.5 + + + + + + + 1 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + -0.5 1.5 0.25 0 0 0 + + + + 1 0.15 0.5 + + + + + + + 1 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + -1.2 0.092 0.25 0 0 -1.5708 + + + + 1 0.15 0.5 + + + + + + + 1 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/textures/wood.png b/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/textures/wood.png new file mode 100644 index 00000000..8111b0c2 Binary files /dev/null and b/turtlebot3_gazebo/models/turtlebot3_dqn_world/inner_walls/textures/wood.png differ diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/model.config b/turtlebot3_gazebo/models/turtlebot3_dqn_world/model.config new file mode 100644 index 00000000..01c16e98 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/model.config @@ -0,0 +1,15 @@ + + + turtlebot3_dqn_world + 1.0 + model.sdf + + + Ryan Shim + jhshim@robotis.com + + + + TurtleBot3 DQN World + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/model.sdf b/turtlebot3_gazebo/models/turtlebot3_dqn_world/model.sdf new file mode 100644 index 00000000..f72b5345 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/model.sdf @@ -0,0 +1,128 @@ + + + + 0 0 0 0 0 0 + true + + + 2.425 0 0.25 0 0 1.5708 + + + + 5 0.15 0.5 + + + + + 0 0 0 0 0 0 + + + 5 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + 0 2.425 0.25 0 0 0 + + + + 5 0.15 0.5 + + + + + 0 0 0 0 0 0 + + + 5 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + -2.425 0 0.25 0 0 1.5708 + + + + 5 0.15 0.5 + + + + + 0 0 0 0 0 0 + + + 5 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + 0 -2.425 0.25 0 0 0 + + + + 5 0.15 0.5 + + + + + 0 0 0 0 0 0 + + + 5 0.15 0.5 + + + + + + textures/wood.png + + + 1 1 1 1 + 1 1 1 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle1/model.config b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle1/model.config new file mode 100644 index 00000000..34e11cd1 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle1/model.config @@ -0,0 +1,15 @@ + + + obstacle1 + 1.0 + model.sdf + + + Ryan Shim + jhshim@robotis.com + + + + TurtleBot3 DQN World + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle1/model.sdf b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle1/model.sdf new file mode 100644 index 00000000..20c6906a --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle1/model.sdf @@ -0,0 +1,31 @@ + + + + 2 2 0.25 0 0 0 + false + + + + + 0.12 + 0.25 + + + + + + + 0.12 + 0.25 + + + + + + + + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle2/model.config b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle2/model.config new file mode 100644 index 00000000..c80563c9 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle2/model.config @@ -0,0 +1,15 @@ + + + obstacle2 + 1.0 + model.sdf + + + Ryan Shim + jhshim@robotis.com + + + + TurtleBot3 DQN World + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle2/model.sdf b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle2/model.sdf new file mode 100644 index 00000000..dee19e00 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacle2/model.sdf @@ -0,0 +1,32 @@ + + + + -2 -2 0.25 0 0 0 + false + + + + + 0.12 + 0.25 + + + + + + + + 0.12 + 0.25 + + + + + + + + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacles/model.config b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacles/model.config new file mode 100644 index 00000000..dc54c5c2 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacles/model.config @@ -0,0 +1,15 @@ + + + obstacles + 1.0 + model.sdf + + + Ryan Shim + jhshim@robotis.com + + + + TurtleBot3 DQN World + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacles/model.sdf b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacles/model.sdf new file mode 100644 index 00000000..b85a0955 --- /dev/null +++ b/turtlebot3_gazebo/models/turtlebot3_dqn_world/obstacles/model.sdf @@ -0,0 +1,108 @@ + + + + 0 0 0 0 0 0 + false + + + -1.0 -1.0 0.25 0 0 0 + + + + 0.15 + 0.5 + + + + + + + 0.15 + 0.5 + + + + + + + + + -1.0 1.0 0.25 0 0 0 + + + + 0.15 + 0.5 + + + + + + + 0.15 + 0.5 + + + + + + + + + 1.0 -1.0 0.25 0 0 0 + + + + 0.15 + 0.5 + + + + + + + 0.15 + 0.5 + + + + + + + + + 1.0 1.0 0.25 0 0 0 + + + + 0.15 + 0.5 + + + + + + + 0.15 + 0.5 + + + + + + + + + diff --git a/turtlebot3_gazebo/models/turtlebot3_dqn_world/textures/wood.png b/turtlebot3_gazebo/models/turtlebot3_dqn_world/textures/wood.png new file mode 100644 index 00000000..8111b0c2 Binary files /dev/null and b/turtlebot3_gazebo/models/turtlebot3_dqn_world/textures/wood.png differ diff --git a/turtlebot3_gazebo/models/turtlebot3_waffle/model.config b/turtlebot3_gazebo/models/turtlebot3_waffle/model.config index b7a7c77d..70afb5d5 100644 --- a/turtlebot3_gazebo/models/turtlebot3_waffle/model.config +++ b/turtlebot3_gazebo/models/turtlebot3_waffle/model.config @@ -4,7 +4,7 @@ TurtleBot3(Waffle) 2.0 model-1_4.sdf - model.sdf + model.sdf Taehun Lim(Darby) diff --git a/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf b/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf index 1ec98357..b4c44ca7 100644 --- a/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf +++ b/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf @@ -479,25 +479,25 @@ - - wheel_left_joint - wheel_right_joint + + wheel_left_joint + wheel_right_joint - - 0.287 - 0.033 + + 0.287 + 0.033 - - 1.0 + + 1.0 - cmd_vel + cmd_vel - odom - odom - base_footprint - 30 + odom + odom + base_footprint + 30 - /tf + /tf diff --git a/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.config b/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.config index 2730a6d0..cbbd1516 100644 --- a/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.config +++ b/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.config @@ -4,7 +4,7 @@ TurtleBot3(Waffle Pi) 2.0 model-1_4.sdf - model.sdf + model.sdf Taehun Lim(Darby) diff --git a/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf b/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf index 911ff42a..b898477a 100644 --- a/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf +++ b/turtlebot3_gazebo/models/turtlebot3_waffle_pi/model.sdf @@ -1,5 +1,5 @@ - + 0.0 0.0 0.0 0.0 0.0 0.0 @@ -139,7 +139,7 @@ 10 scan base_scan - + 360 @@ -158,7 +158,7 @@ 0.0 0.01 - + @@ -479,25 +479,25 @@ - - wheel_left_joint - wheel_right_joint + + wheel_left_joint + wheel_right_joint - - 0.287 - 0.033 + + 0.287 + 0.033 - - 1.0 + + 1.0 - cmd_vel + cmd_vel - odom - odom - base_footprint - 30 + odom + odom + base_footprint + 30 - /tf + /tf diff --git a/turtlebot3_gazebo/package.xml b/turtlebot3_gazebo/package.xml index 39aed027..fdbc6acd 100644 --- a/turtlebot3_gazebo/package.xml +++ b/turtlebot3_gazebo/package.xml @@ -2,7 +2,7 @@ turtlebot3_gazebo - 2.3.3 + 2.3.5 Gazebo simulation package for the TurtleBot3 @@ -22,6 +22,9 @@ ros_gz_bridge ros_gz_image ros_gz_sim + gz_sim_vendor + gz_plugin_vendor + gz_math_vendor sensor_msgs tf2 diff --git a/turtlebot3_gazebo/src/obstacle1.cpp b/turtlebot3_gazebo/src/obstacle1.cpp index 63540ade..d5880c2c 100644 --- a/turtlebot3_gazebo/src/obstacle1.cpp +++ b/turtlebot3_gazebo/src/obstacle1.cpp @@ -12,53 +12,84 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Ryan Shim +// Author: Ryan Shim, ChanHyeong Lee #include "turtlebot3_gazebo/obstacle1.hpp" -namespace gazebo +#include +#include +#include + +namespace turtlebot3_gazebo { -void Obstacle1::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) + +void Obstacle1Plugin::Configure( + const gz::sim::Entity & entity, + const std::shared_ptr &, + gz::sim::EntityComponentManager &, + gz::sim::EventManager &) { - this->model = _parent; + this->model = gz::sim::Model(entity); + this->startTime = std::chrono::steady_clock::now(); - gazebo::common::PoseAnimationPtr anim( - new gazebo::common::PoseAnimation("move1", 160.0, true)); + this->waypoints = { + {2.0, 2.0, 0.25}, + {1.5, 1.0, 0.25}, + {-1.5, 1.0, 0.25}, + {-1.7, -1.0, 0.25}, + {-1.5, 1.0, 0.25}, + {1.5, 1.0, 0.25}, + {2.0, 2.0, 0.25} + }; - gazebo::common::PoseKeyFrame * key; + this->segmentDistances.clear(); + this->totalDistance = 0.0; - key = anim->CreateKeyFrame(0); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + for (size_t i = 0; i < waypoints.size() - 1; ++i) { + double dist = (waypoints[i + 1] - waypoints[i]).Length(); + this->segmentDistances.push_back(dist); + this->totalDistance += dist; + } +} - key = anim->CreateKeyFrame(10); - key->Translation(ignition::math::Vector3d(-0.5, -1.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); +void Obstacle1Plugin::PreUpdate( + const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager & ecm) +{ + if (!this->model.Valid(ecm)) {return;} - key = anim->CreateKeyFrame(50); - key->Translation(ignition::math::Vector3d(-3.5, -1.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - this->startTime; - key = anim->CreateKeyFrame(70); - key->Translation(ignition::math::Vector3d(-3.7, -3.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + double travelDist = std::fmod(elapsed.count() * this->speed, this->totalDistance); - key = anim->CreateKeyFrame(90); - key->Translation(ignition::math::Vector3d(-3.5, -1.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + size_t idx = 0; + double acc = 0.0; - key = anim->CreateKeyFrame(130); - key->Translation(ignition::math::Vector3d(-0.5, -1.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + while (idx < segmentDistances.size() && acc + segmentDistances[idx] < travelDist) { + acc += segmentDistances[idx]; + ++idx; + } - key = anim->CreateKeyFrame(140); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + if (idx >= segmentDistances.size()) {return;} - key = anim->CreateKeyFrame(160); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + double localT = (travelDist - acc) / segmentDistances[idx]; + gz::math::Vector3d start = waypoints[idx]; + gz::math::Vector3d end = waypoints[idx + 1]; + gz::math::Vector3d currentPos = start + (end - start) * localT; - _parent->SetAnimation(anim); + gz::math::Pose3d pose(currentPos, gz::math::Quaterniond::Identity); + this->model.SetWorldPoseCmd(ecm, pose); } -} // namespace gazebo + +} // namespace turtlebot3_gazebo + +GZ_ADD_PLUGIN( + turtlebot3_gazebo::Obstacle1Plugin, + gz::sim::System, + gz::sim::ISystemConfigure, + gz::sim::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS( + turtlebot3_gazebo::Obstacle1Plugin, + "turtlebot3_gazebo::Obstacle1Plugin") diff --git a/turtlebot3_gazebo/src/obstacle2.cpp b/turtlebot3_gazebo/src/obstacle2.cpp index 967e2825..c89e8581 100644 --- a/turtlebot3_gazebo/src/obstacle2.cpp +++ b/turtlebot3_gazebo/src/obstacle2.cpp @@ -12,69 +12,88 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Ryan Shim +// Author: Ryan Shim, ChanHyeong Lee #include "turtlebot3_gazebo/obstacle2.hpp" -namespace gazebo -{ -void Obstacle2::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) -{ - this->model = _parent; - - gazebo::common::PoseAnimationPtr anim( - new gazebo::common::PoseAnimation("move2", 140.0, true)); +#include +#include +#include - gazebo::common::PoseKeyFrame * key; - - key = anim->CreateKeyFrame(0); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); +namespace turtlebot3_gazebo +{ - key = anim->CreateKeyFrame(10); - key->Translation(ignition::math::Vector3d(0.7, 0.2, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); +void Obstacle2Plugin::Configure( + const gz::sim::Entity & entity, + const std::shared_ptr &, + gz::sim::EntityComponentManager &, + gz::sim::EventManager &) +{ + this->model = gz::sim::Model(entity); + this->startTime = std::chrono::steady_clock::now(); + + this->waypoints = { + {-2.0, -2.0, 0.25}, + {-1.3, -1.8, 0.25}, + {0.5, 2.0, 0.25}, + {-2.0, 1.5, 0.25}, + {1.5, -0.2, 0.25}, + {1.5, -2.0, 0.25}, + {0.0, -1.5, 0.25}, + {-0.5, -1.0, 0.25}, + {-1.0, -1.5, 0.25}, + {-1.5, -1.9, 0.25}, + {-2.0, -2.0, 0.25} + }; + + this->segmentDistances.clear(); + this->totalDistance = 0.0; + + for (size_t i = 0; i < waypoints.size() - 1; ++i) { + double dist = (waypoints[i + 1] - waypoints[i]).Length(); + this->segmentDistances.push_back(dist); + this->totalDistance += dist; + } +} - key = anim->CreateKeyFrame(40); - key->Translation(ignition::math::Vector3d(2.5, 3.5, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); +void Obstacle2Plugin::PreUpdate( + const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager & ecm) +{ + if (!this->model.Valid(ecm)) {return;} - key = anim->CreateKeyFrame(55); - key->Translation(ignition::math::Vector3d(0.3, 3.5, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - this->startTime; - key = anim->CreateKeyFrame(85); - key->Translation(ignition::math::Vector3d(3.5, 1.8, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + double travelDist = std::fmod(elapsed.count() * this->speed, this->totalDistance); - key = anim->CreateKeyFrame(100); - key->Translation(ignition::math::Vector3d(3.5, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + size_t idx = 0; + double acc = 0.0; - key = anim->CreateKeyFrame(110); - key->Translation(ignition::math::Vector3d(2.0, 0.5, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + while (idx < segmentDistances.size() && acc + segmentDistances[idx] < travelDist) { + acc += segmentDistances[idx]; + ++idx; + } - key = anim->CreateKeyFrame(115); - key->Translation(ignition::math::Vector3d(1.5, 1.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + if (idx >= segmentDistances.size()) {return;} - key = anim->CreateKeyFrame(120); - key->Translation(ignition::math::Vector3d(1.0, 0.5, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + double localT = (travelDist - acc) / segmentDistances[idx]; + gz::math::Vector3d start = waypoints[idx]; + gz::math::Vector3d end = waypoints[idx + 1]; + gz::math::Vector3d currentPos = start + (end - start) * localT; - key = anim->CreateKeyFrame(125); - key->Translation(ignition::math::Vector3d(0.5, 0.1, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + gz::math::Pose3d pose(currentPos, gz::math::Quaterniond::Identity); + this->model.SetWorldPoseCmd(ecm, pose); +} - key = anim->CreateKeyFrame(130); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); +} // namespace turtlebot3_gazebo - key = anim->CreateKeyFrame(140); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); +GZ_ADD_PLUGIN( + turtlebot3_gazebo::Obstacle2Plugin, + gz::sim::System, + gz::sim::ISystemConfigure, + gz::sim::ISystemPreUpdate) - _parent->SetAnimation(anim); -} -} // namespace gazebo +GZ_ADD_PLUGIN_ALIAS( + turtlebot3_gazebo::Obstacle2Plugin, + "turtlebot3_gazebo::Obstacle2Plugin") diff --git a/turtlebot3_gazebo/src/obstacles.cpp b/turtlebot3_gazebo/src/obstacles.cpp index 2bdfefaa..cd16712d 100644 --- a/turtlebot3_gazebo/src/obstacles.cpp +++ b/turtlebot3_gazebo/src/obstacles.cpp @@ -12,33 +12,59 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// Author: Ryan Shim +// Author: Ryan Shim, ChanHyeong Lee #include "turtlebot3_gazebo/obstacles.hpp" -namespace gazebo +#include +#include + +#include +#include +#include +#include + +namespace turtlebot3_gazebo +{ + +void ObstaclesPlugin::Configure( + const gz::sim::Entity & entity, + const std::shared_ptr &, + gz::sim::EntityComponentManager &, + gz::sim::EventManager &) { -void Obstacles::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) + this->model = gz::sim::Model(entity); + this->startTime = std::chrono::steady_clock::now(); +} + +void ObstaclesPlugin::PreUpdate( + const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager & ecm) { - this->model = _parent; + if (!this->model.Valid(ecm)) { + return; + } - gazebo::common::PoseAnimationPtr anim( - new gazebo::common::PoseAnimation("move", 40.0, true)); + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - this->startTime; + double t = fmod(elapsed.count(), 80.0); + double angle = 2 * M_PI * t / 80.0; - gazebo::common::PoseKeyFrame * key; + gz::math::Pose3d pose( + gz::math::Vector3d(0, 0, 0), + gz::math::Quaterniond(0, 0, angle)); - key = anim->CreateKeyFrame(0); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 0)); + this->model.SetWorldPoseCmd(ecm, pose); +} - key = anim->CreateKeyFrame(20); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, PI)); +} // namespace turtlebot3_gazebo - key = anim->CreateKeyFrame(40); - key->Translation(ignition::math::Vector3d(0.0, 0.0, 0.0)); - key->Rotation(ignition::math::Quaterniond(0, 0, 2 * PI)); +GZ_ADD_PLUGIN( + turtlebot3_gazebo::ObstaclesPlugin, + gz::sim::System, + gz::sim::ISystemConfigure, + gz::sim::ISystemPreUpdate) - _parent->SetAnimation(anim); -} -} // namespace gazebo +GZ_ADD_PLUGIN_ALIAS( + turtlebot3_gazebo::ObstaclesPlugin, + "turtlebot3_gazebo::ObstaclesPlugin") diff --git a/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage1.world b/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage1.world index b6225600..7e7b4dce 100644 --- a/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage1.world +++ b/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage1.world @@ -1,26 +1,24 @@ - - + + - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - + + + + + + + + ogre2 + 1000.0 @@ -42,13 +40,30 @@ - - - 1 - - model://turtlebot3_dqn_world - - + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + + + false + 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 + + + + true + + model://turtlebot3_dqn_world + + diff --git a/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage2.world b/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage2.world index 8efeee78..36646ce8 100644 --- a/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage2.world +++ b/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage2.world @@ -1,26 +1,28 @@ - - + + - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - + + + + + + + + ogre2 + + + 1000.0 @@ -43,13 +45,26 @@ - - 1 - - model://turtlebot3_dqn_world - - + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + + + false + 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 + + + model://turtlebot3_dqn_world + model://turtlebot3_dqn_world/obstacles diff --git a/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage3.world b/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage3.world index d8d7dc1a..e4f9873b 100644 --- a/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage3.world +++ b/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage3.world @@ -1,26 +1,28 @@ - - + + - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - + + + + + + + + ogre2 + + + 1000.0 @@ -43,20 +45,31 @@ - - 1 - - model://turtlebot3_dqn_world - - + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + - - - 1 - - model://turtlebot3_dqn_world/obstacles - - + + false + 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 + + + + model://turtlebot3_dqn_world + + + model://turtlebot3_dqn_world/obstacles + + diff --git a/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage4.world b/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage4.world index b0174dca..861d981f 100644 --- a/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage4.world +++ b/turtlebot3_gazebo/worlds/turtlebot3_dqn_stage4.world @@ -1,26 +1,28 @@ - - + + - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - + + + + + + + + ogre2 + + + 1000.0 @@ -43,33 +45,38 @@ - - 1 - - model://turtlebot3_dqn_world - - + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane + + + false + 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 + + + + model://turtlebot3_dqn_world + - 0 0 0 0 0 0 model://turtlebot3_dqn_world/inner_walls - - - - - 2 2 0 0 0 0 - model://turtlebot3_dqn_world/obstacle1 - - - - - - - -2 -2 0 0 0 0 - model://turtlebot3_dqn_world/obstacle2 - - + + model://turtlebot3_dqn_world/obstacle1 + + + + model://turtlebot3_dqn_world/obstacle2 + + diff --git a/turtlebot3_simulations/CHANGELOG.rst b/turtlebot3_simulations/CHANGELOG.rst index c554d72d..5de2941a 100644 --- a/turtlebot3_simulations/CHANGELOG.rst +++ b/turtlebot3_simulations/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package turtlebot3_simulations ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.5 (2025-06-04) +------------------ +* Added turtlebot3_machine_learning world and plugin +* Contributors: ChanHyeong Lee + 2.3.3 (2025-05-29) ------------------ * Deprecate ament_include_dependency usage in CMakeLists.txt, related PR(https://github.com/ROBOTIS-GIT/turtlebot3_simulations/pull/234) diff --git a/turtlebot3_simulations/package.xml b/turtlebot3_simulations/package.xml index f338fb40..e71b3518 100644 --- a/turtlebot3_simulations/package.xml +++ b/turtlebot3_simulations/package.xml @@ -2,7 +2,7 @@ turtlebot3_simulations - 2.3.3 + 2.3.5 ROS 2 packages for TurtleBot3 simulations