Update simulation for machine learning jazzy version

turtlebot3_gazebo/CMakeLists.txt

@@ -50,25 +50,32 @@ 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})
+ament_target_dependencies(turtlebot3_drive ${dependencies})
 
-# 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_include_directories(${lib}
+    PUBLIC ${gz-sim8_INCLUDE_DIRS}
+  )
+  target_link_libraries(${lib}
+    ${gz-sim8_LIBRARIES}
+  )
+endforeach()
 
 ################################################################################
 # Install
 ################################################################################
-install(TARGETS ${EXEC_NAME}
+install(TARGETS
+  ${EXEC_NAME}
+  obstacles
+  obstacle1
+  obstacle2
   DESTINATION lib/${PROJECT_NAME}
 )
 

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 <ignition/math.hh>
-#include <gazebo/common/common.hh>
-#include <gazebo/gazebo.hh>
-#include <gazebo/physics/physics.hh>
+#include <chrono>
+#include <memory>
+#include <vector>
 
-namespace gazebo
+#include <gz/sim/System.hh>
+#include <gz/sim/Model.hh>
+#include <gz/math/Vector3.hh>
+
+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<const sdf::Element> & 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<gz::math::Vector3d> waypoints;
+  std::vector<double> 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_

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 <ignition/math.hh>
-#include <gazebo/common/common.hh>
-#include <gazebo/gazebo.hh>
-#include <gazebo/physics/physics.hh>
+#include <chrono>
+#include <memory>
+#include <vector>
 
-namespace gazebo
+#include <gz/sim/System.hh>
+#include <gz/sim/Model.hh>
+#include <gz/math/Vector3.hh>
+
+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<const sdf::Element> & 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<gz::math::Vector3d> waypoints;
+  std::vector<double> 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_

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 <ignition/math.hh>
-#include <gazebo/common/common.hh>
-#include <gazebo/gazebo.hh>
-#include <gazebo/physics/physics.hh>
+#include <chrono>
+#include <memory>
 
-#define PI 3.141592
+#include <gz/sim/components/Pose.hh>
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/EventManager.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/System.hh>
 
-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<const sdf::Element> & 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_

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