Migrate followbot to ROS2

followbot/CMakeLists.txt

@@ -1,147 +1,59 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(followbot)
-find_package(catkin REQUIRED COMPONENTS
-  rospy
-)
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependencies might have been
-##     pulled in transitively but can be declared for certainty nonetheless:
-##     * add a build_depend tag for "message_generation"
-##     * add a run_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
 
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES chessbot
-#  CATKIN_DEPENDS rospy
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
+# if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+#   add_compile_options(-Wall -Wextra -Wpedantic)
+# endif()
 
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-# include_directories(include)
-include_directories(
-  ${catkin_INCLUDE_DIRS}
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+# Install directories
+ install(DIRECTORY
+   models launch
+   DESTINATION share/${PROJECT_NAME}
+ )
+
+# Install Python package
+ament_python_install_package(
+    followbot
+    PACKAGE_DIR python/followbot)
+
+# Install Python scripts
+install(PROGRAMS
+    scripts/follower_p.py
+#    scripts/test_cmd_vel.py
+    scripts/test_move_base.py
+    scripts/test_vision.py
+    DESTINATION lib/${PROJECT_NAME}
 )
 
-## Declare a cpp library
-# add_library(chessbot
-#   src/${PROJECT_NAME}/chessbot.cpp
-# )
-
-## Declare a cpp executable
-# add_executable(chessbot_node src/chessbot_node.cpp)
-
-## Add cmake target dependencies of the executable/library
-## as an example, message headers may need to be generated before nodes
-# add_dependencies(chessbot_node chessbot_generate_messages_cpp)
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(chessbot_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS chessbot chessbot_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# Install C++ nodes
+# set(AUTONOMY_SOURCES
+#     src/navigation_behaviors.cpp 
+#     src/vision_behaviors.cpp
 # )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+set(TARGET_DEPENDS
+    nav2_msgs sensor_msgs cv_bridge image_transport
+    tf2 tf2_ros tf2_geometry_msgs
+)
+include_directories(include)
+# add_executable(autonomy_node_cpp src/autonomy_node.cpp ${AUTONOMY_SOURCES})
+# install(TARGETS
+#     autonomy_node_cpp
+#     DESTINATION lib/${PROJECT_NAME}
 # )
+#ament_target_dependencies(${TARGET_DEPENDS})
+#target_link_libraries(${OpenCV_LIBS} ${YAML_CPP_LIBRARIES})
 
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_chessbot.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+ament_package()

followbot/launch/course.launch.py

@@ -0,0 +1,111 @@
+from os.path import join
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
+    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    x_pose = LaunchConfiguration('x_pose', default='-1.75')
+    y_pose = LaunchConfiguration('y_pose', default='0.2')
+    yaw = LaunchConfiguration('yaw', default='1.57')
+
+    world = os.path.join(
+        get_package_share_directory('turtlebot3_gazebo'),
+        'worlds',
+        'Circuit.world'
+    )
+
+    gzserver_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
+        ),
+        launch_arguments={'world': world}.items()
+    )
+
+    gzclient_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
+        )
+    )
+
+    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,
+            'yaw': yaw
+        }.items()
+    )
+
+    return LaunchDescription(
+        [
+            # Arguments
+            DeclareLaunchArgument(
+                "location_file",
+                default_value=TextSubstitution(text=default_world_dir),
+                description="YAML file name containing map locations in the world.",
+            ),
+            DeclareLaunchArgument(
+                "target_color",
+                default_value=TextSubstitution(text="blue"),
+                description="Target object color (red, green, or blue)",
+            ),
+            DeclareLaunchArgument(
+                "tree_type",
+                default_value=TextSubstitution(text="queue"),
+                description="Behavior tree type (naive or queue)",
+            ),
+            DeclareLaunchArgument(
+                "enable_vision",
+                default_value=TextSubstitution(text="True"),
+                description="Enable vision behaviors. If false, do navigation only.",
+            ),
+            # Main follower node
+            Node(
+                package="followbot",
+                executable="follower.py",
+                name="follower_node_python",
+                output="screen",
+                emulate_tty=True,
+                parameters=[
+                    {
+                        "use_sim_time": LaunchConfiguration("use_sim_time"),
+                        "debug": LaunchConfiguration("debug"),
+                        "world_name": LaunchConfiguration("world_name"),
+                    }
+                ],
+            ),
+            # Behavior tree visualization
+            ExecuteProcess(cmd=["py-trees-tree-viewer", "--no-sandbox"]),
+        ]
+    )
+
+# <launch>
+
+#   <!-- Fake laser -->
+#   <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
+#   <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
+#         args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
+#     <param name="scan_height" value="10"/>
+#     <param name="output_frame_id" value="/camera_depth_frame"/>
+#     <param name="range_min" value="0.45"/>
+#     <remap from="image" to="/camera/depth/image_raw"/>
+#     <remap from="scan" to="/scan"/>
+#   </node>
+# </launch>
+