From e154bdce5deaa98dc6cc57b99ffc0f1b13f1196b Mon Sep 17 00:00:00 2001 From: gautz <64833674+gautz@users.noreply.github.com> Date: Tue, 17 Feb 2026 16:42:34 +0100 Subject: [PATCH 1/4] restructure folders --- followbot/{ => launch}/course.launch | 0 .../course/materials/scripts}/course.material | 0 .../course/materials/textures}/course.png | Bin followbot/{ => models}/line.png | Bin followbot/{ => models}/line_finder.png | Bin followbot/{ => models}/line_hsv.png | Bin followbot/{ => models}/line_masked.png | Bin followbot/{ => python/followbot}/follower.py | 0 followbot/{ => python/followbot}/follower_blind.py | 0 .../{ => python/followbot}/follower_color_filter.py | 0 .../{ => python/followbot}/follower_line_finder.py | 0 followbot/{ => python/followbot}/follower_opencv.py | 0 followbot/{ => scripts}/follower_p.py | 0 followbot/{ => worlds}/course.world | 0 14 files changed, 0 insertions(+), 0 deletions(-) rename followbot/{ => launch}/course.launch (100%) rename followbot/{ => models/course/materials/scripts}/course.material (100%) rename followbot/{ => models/course/materials/textures}/course.png (100%) rename followbot/{ => models}/line.png (100%) rename followbot/{ => models}/line_finder.png (100%) rename followbot/{ => models}/line_hsv.png (100%) rename followbot/{ => models}/line_masked.png (100%) rename followbot/{ => python/followbot}/follower.py (100%) rename followbot/{ => python/followbot}/follower_blind.py (100%) rename followbot/{ => python/followbot}/follower_color_filter.py (100%) rename followbot/{ => python/followbot}/follower_line_finder.py (100%) rename followbot/{ => python/followbot}/follower_opencv.py (100%) rename followbot/{ => scripts}/follower_p.py (100%) rename followbot/{ => worlds}/course.world (100%) diff --git a/followbot/course.launch b/followbot/launch/course.launch similarity index 100% rename from followbot/course.launch rename to followbot/launch/course.launch diff --git a/followbot/course.material b/followbot/models/course/materials/scripts/course.material similarity index 100% rename from followbot/course.material rename to followbot/models/course/materials/scripts/course.material diff --git a/followbot/course.png b/followbot/models/course/materials/textures/course.png similarity index 100% rename from followbot/course.png rename to followbot/models/course/materials/textures/course.png diff --git a/followbot/line.png b/followbot/models/line.png similarity index 100% rename from followbot/line.png rename to followbot/models/line.png diff --git a/followbot/line_finder.png b/followbot/models/line_finder.png similarity index 100% rename from followbot/line_finder.png rename to followbot/models/line_finder.png diff --git a/followbot/line_hsv.png b/followbot/models/line_hsv.png similarity index 100% rename from followbot/line_hsv.png rename to followbot/models/line_hsv.png diff --git a/followbot/line_masked.png b/followbot/models/line_masked.png similarity index 100% rename from followbot/line_masked.png rename to followbot/models/line_masked.png diff --git a/followbot/follower.py b/followbot/python/followbot/follower.py similarity index 100% rename from followbot/follower.py rename to followbot/python/followbot/follower.py diff --git a/followbot/follower_blind.py b/followbot/python/followbot/follower_blind.py similarity index 100% rename from followbot/follower_blind.py rename to followbot/python/followbot/follower_blind.py diff --git a/followbot/follower_color_filter.py b/followbot/python/followbot/follower_color_filter.py similarity index 100% rename from followbot/follower_color_filter.py rename to followbot/python/followbot/follower_color_filter.py diff --git a/followbot/follower_line_finder.py b/followbot/python/followbot/follower_line_finder.py similarity index 100% rename from followbot/follower_line_finder.py rename to followbot/python/followbot/follower_line_finder.py diff --git a/followbot/follower_opencv.py b/followbot/python/followbot/follower_opencv.py similarity index 100% rename from followbot/follower_opencv.py rename to followbot/python/followbot/follower_opencv.py diff --git a/followbot/follower_p.py b/followbot/scripts/follower_p.py similarity index 100% rename from followbot/follower_p.py rename to followbot/scripts/follower_p.py diff --git a/followbot/course.world b/followbot/worlds/course.world similarity index 100% rename from followbot/course.world rename to followbot/worlds/course.world From 492481cfcc69265a3525e39de36ff9f8c3cad2ad Mon Sep 17 00:00:00 2001 From: gautz <64833674+gautz@users.noreply.github.com> Date: Tue, 17 Feb 2026 16:45:45 +0100 Subject: [PATCH 2/4] migrate ros2 --- followbot/CMakeLists.txt | 190 +++++++------------------ followbot/launch/course.launch | 29 ---- followbot/launch/course.launch.py | 111 +++++++++++++++ followbot/models/course/model.config | 16 +++ followbot/models/course/model.sdf | 42 ++++++ followbot/package.xml | 26 ++-- followbot/python/followbot/__init__.py | 0 followbot/setup.py | 47 ++++++ 8 files changed, 284 insertions(+), 177 deletions(-) delete mode 100644 followbot/launch/course.launch create mode 100644 followbot/launch/course.launch.py create mode 100644 followbot/models/course/model.config create mode 100644 followbot/models/course/model.sdf create mode 100644 followbot/python/followbot/__init__.py create mode 100644 followbot/setup.py diff --git a/followbot/CMakeLists.txt b/followbot/CMakeLists.txt index 06de953..9597fee 100644 --- a/followbot/CMakeLists.txt +++ b/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() diff --git a/followbot/launch/course.launch b/followbot/launch/course.launch deleted file mode 100644 index ef0dc93..0000000 --- a/followbot/launch/course.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/followbot/launch/course.launch.py b/followbot/launch/course.launch.py new file mode 100644 index 0000000..602ac1b --- /dev/null +++ b/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"]), + ] + ) + +# + +# +# +# +# +# +# +# +# +# +# + diff --git a/followbot/models/course/model.config b/followbot/models/course/model.config new file mode 100644 index 0000000..725e5f6 --- /dev/null +++ b/followbot/models/course/model.config @@ -0,0 +1,16 @@ + + + + course + 1.0 + model.sdf + + sdq + test@gmail.com + + + + A simple textured ground plane + + + diff --git a/followbot/models/course/model.sdf b/followbot/models/course/model.sdf new file mode 100644 index 0000000..2afe8d4 --- /dev/null +++ b/followbot/models/course/model.sdf @@ -0,0 +1,42 @@ + + + + true + + + + + 0 0 1 + 4 4 + + + + + + 100 + 50 + + + + + + false + + + 0 0 1 + 4 4 + + + + + + + + + + diff --git a/followbot/package.xml b/followbot/package.xml index e96f264..8b28acb 100644 --- a/followbot/package.xml +++ b/followbot/package.xml @@ -1,16 +1,24 @@ - + followbot - 0.0.0 + 2.0.0 The followbot package + Morgan Quigley - BSD - Morgan Quigley - catkin - rospy - rospy - gazebo_ros + MIT + + + + rclpy + nav2_msgs + sensor_msgs + tb3_worlds + tf2 + tf2_ros + + - + ament_cmake diff --git a/followbot/python/followbot/__init__.py b/followbot/python/followbot/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/followbot/setup.py b/followbot/setup.py new file mode 100644 index 0000000..fdb5603 --- /dev/null +++ b/followbot/setup.py @@ -0,0 +1,47 @@ +from setuptools import find_packages +from setuptools import setup + +package_name = 'followbot' + +setup( + name=package_name, + version='2.0.0', + packages=find_packages(), + data_files=[ + # ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + # To be added + # ('share/' + package_name + '/launch', glob.glob(os.path.join('launch', + # 'turtlebot3_interactive_marker.launch.py'))), + # ('share/' + package_name + '/launch', glob.glob(os.path.join('launch', + # 'turtlebot3_obstacle_detection.launch.py'))), + # ('share/' + package_name + '/rviz', glob.glob(os.path.join('rviz', + # 'turtlebot3_interactive_marker.rviz'))), + ], + install_requires=['setuptools', 'launch'], + zip_safe=True, + author=['Ryan Shim', 'Gilbert'], + author_email=['jhshim@robotis.com', 'kkjong@robotis.com'], + maintainer='Will Son', + maintainer_email='willson@robotis.com', + keywords=['ROS', 'ROS2', 'examples', 'rclpy'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: Apache Software License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description=( + 'Examples of Different TurtleBot3 Usage.' + ), + license='Apache License, Version 2.0', + entry_points={ + 'console_scripts': [ + # To be added + # 'turtlebot3_interactive_marker = \ + # turtlebot3_example.turtlebot3_interactive_marker.main:main', + 'followbot = \ + followbot.follower_p:Follower', + ], + }, +) From 6cc3d8201b605d303c9cf91b49a27a185f09ecb3 Mon Sep 17 00:00:00 2001 From: gautz <64833674+gautz@users.noreply.github.com> Date: Tue, 17 Feb 2026 16:45:58 +0100 Subject: [PATCH 3/4] migrate follower_p --- followbot/scripts/follower_p.py | 80 ++++++++++++++++++++++++--------- 1 file changed, 60 insertions(+), 20 deletions(-) diff --git a/followbot/scripts/follower_p.py b/followbot/scripts/follower_p.py index 3360f8a..cd6de58 100755 --- a/followbot/scripts/follower_p.py +++ b/followbot/scripts/follower_p.py @@ -1,45 +1,85 @@ #!/usr/bin/env python # BEGIN ALL -import rospy, cv2, cv_bridge, numpy +import cv2, cv_bridge, numpy from sensor_msgs.msg import Image from geometry_msgs.msg import Twist -class Follower: +import sys +import termios +# import os +import yaml +import random +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from nav_msgs.msg import Odometry +import time +# import py_trees +# import py_trees_ros +# from py_trees.common import OneShotPolicy +# from ament_index_python.packages import get_package_share_directory + +# from tb3_behaviors.navigation import GoToPose, GetLocationFromQueue +# from tb3_behaviors.vision import LookForObject + + +# default_location_file = os.path.join( +# get_package_share_directory("tb3_worlds"), "maps", "sim_house_locations.yaml" +# ) + +class Follower(Node): def __init__(self): + super().__init__("follower_node") + self.sub = self.create_subscription( + Image, "/image_raw", self.img_callback, 10 + ) self.bridge = cv_bridge.CvBridge() cv2.namedWindow("window", 1) - self.image_sub = rospy.Subscriber('camera/rgb/image_raw', - Image, self.image_callback) - self.cmd_vel_pub = rospy.Publisher('cmd_vel_mux/input/teleop', - Twist, queue_size=1) + qos = QoSProfile(depth=1) + self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', qos) self.twist = Twist() - def image_callback(self, msg): - image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8') - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + + def img_callback(self, msg): + twist = Twist() + img = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8') + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) lower_yellow = numpy.array([ 10, 10, 10]) upper_yellow = numpy.array([255, 255, 250]) mask = cv2.inRange(hsv, lower_yellow, upper_yellow) - h, w, d = image.shape + h = int(img.shape[0]) + w = int(img.shape[1]) + d = int(img.shape[2]) + self.get_logger().info(f"img.shape [h: {h}, w: {w}, d: {d}] ...") search_top = 3*h/4 search_bot = 3*h/4 + 20 - mask[0:search_top, 0:w] = 0 - mask[search_bot:h, 0:w] = 0 + self.get_logger().info(f"search_top: {search_top}, search_bot: {search_bot}") + mask[0:int(search_top), 0:w] = 0 + mask[int(search_bot):h, 0:w] = 0 M = cv2.moments(mask) if M['m00'] > 0: cx = int(M['m10']/M['m00']) cy = int(M['m01']/M['m00']) - cv2.circle(image, (cx, cy), 20, (0,0,255), -1) + cv2.circle(img, (cx, cy), 20, (0,0,255), -1) # BEGIN CONTROL err = cx - w/2 - self.twist.linear.x = 0.2 - self.twist.angular.z = -float(err) / 100 - self.cmd_vel_pub.publish(self.twist) + twist.linear.x = 0.2 + twist.angular.z = -float(err) / 100 + self.cmd_vel_pub.publish(twist) # END CONTROL - cv2.imshow("window", image) + cv2.imshow("window", img) cv2.waitKey(3) -rospy.init_node('follower') -follower = Follower() -rospy.spin() +# rospy.init_node('follower') +# follower = Follower() +# rospy.spin() # END ALL + +if __name__ == "__main__": + rclpy.init() + follower = Follower() + rclpy.spin(follower) + # follower.execute() + follower.destroy_node() + rclpy.shutdown() + From 0511fd7e40f59fd6168d5b4aa819f509c0fd8a0c Mon Sep 17 00:00:00 2001 From: gautz <64833674+gautz@users.noreply.github.com> Date: Tue, 17 Feb 2026 16:46:08 +0100 Subject: [PATCH 4/4] add tests --- followbot/scripts/test_move_base.py | 54 ++++++++++++++++++ followbot/scripts/test_vision.py | 88 +++++++++++++++++++++++++++++ 2 files changed, 142 insertions(+) create mode 100755 followbot/scripts/test_move_base.py create mode 100755 followbot/scripts/test_vision.py diff --git a/followbot/scripts/test_move_base.py b/followbot/scripts/test_move_base.py new file mode 100755 index 0000000..b74ed8d --- /dev/null +++ b/followbot/scripts/test_move_base.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 + +""" +Script that tests the `navigate_to_pose` action client to move the robot base. + +Example usage: + Python: python3 test_move_base.py --x 0 --y 2 --theta 1.57 + ros2: ros2 run tb3_autonomy test_move_base.py --x 0 --y 2 --theta 1.57 +""" + +import argparse +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +import transforms3d +from nav2_msgs.action import NavigateToPose + + +class MoveBaseClient(Node): + def __init__(self): + super().__init__("move_base_client") + self.cli = ActionClient(self, NavigateToPose, "/navigate_to_pose") + self.get_logger().info("Move base test node started") + + def send_pose_goal(self, x, y, theta): + self.get_logger().info(f"Going to [x: {x}, y: {y}, theta: {theta}] ...") + goal_msg = NavigateToPose.Goal() + goal_msg.pose.pose.position.x = x + goal_msg.pose.pose.position.y = y + quat = transforms3d.euler.euler2quat(0, 0, theta) + goal_msg.pose.pose.orientation.w = quat[0] + goal_msg.pose.pose.orientation.x = quat[1] + goal_msg.pose.pose.orientation.y = quat[2] + goal_msg.pose.pose.orientation.z = quat[3] + self.cli.wait_for_server() + return self.cli.send_goal_async(goal_msg) + + +if __name__ == "__main__": + # Parse command-line arguments + parser = argparse.ArgumentParser(description="Move base test script") + parser.add_argument("--x", type=float, default=1.0) + parser.add_argument("--y", type=float, default=0.0) + parser.add_argument("--theta", type=float, default=0.0) + args = parser.parse_args() + + # Start ROS node and action client + rclpy.init() + client = MoveBaseClient() + + # Send goal to the move_base action server + future = client.send_pose_goal(args.x, args.y, args.theta) + rclpy.spin_until_future_complete(client, future) + client.get_logger("Pose goal reached.") diff --git a/followbot/scripts/test_vision.py b/followbot/scripts/test_vision.py new file mode 100755 index 0000000..3629273 --- /dev/null +++ b/followbot/scripts/test_vision.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 + +""" +Script that tests a simple color thresholding vision approach. + +Example usage: + Python: python3 test_vision.py --min_h 0 --max_h 30 --min_s 220 --max_s 255 --min_v 0 --max_v 255 + ros2: ros2 run tb3_autonomy test_vision.py --min_h 0 --max_h 30 --min_s 220 --max_s 255 --min_v 0 --max_v 255 + +For reference, these are some approximate thresholds on the H channel. + Red: 160 - 180 + Green: 40 - 90 + Blue: 100 - 150 + +Since the blocks are highly saturated, the default S channel thresholds are tight (220-255) +""" + +import argparse +import cv_bridge +import cv2 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image + + +class ColorThresholdTester(Node): + def __init__(self, args): + # Define ROS subscriber + super().__init__("test_vision") + self.sub = self.create_subscription( + Image, "/image_raw", self.img_callback, 10 + ) + + # Define vision related variables + self.bridge = cv_bridge.CvBridge() + cv2.namedWindow("Image with Detections", cv2.WINDOW_NORMAL) + params = cv2.SimpleBlobDetector_Params() + params.minArea = 100 + params.maxArea = 100000 + params.filterByArea = True + params.filterByColor = False + params.filterByInertia = False + params.filterByConvexity = False + params.thresholdStep = 60 + self.detector = cv2.SimpleBlobDetector_create(params) + self.min_bounds = (args.min_h, args.min_s, args.min_v) + self.max_bounds = (args.max_h, args.max_s, args.max_v) + self.get_logger().info( + f"Using limits:\n" + f"H: [{args.min_h} {args.max_h}] " + f"S: [{args.min_s} {args.max_s}] " + f"V: [{args.min_v} {args.max_v}] " + ) + + def img_callback(self, msg): + """Image topic subscriber callback""" + img = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(hsv, self.min_bounds, self.max_bounds) + keypoints = self.detector.detect(mask) + labeled_img = cv2.drawKeypoints( + img, + keypoints, + None, + (255, 0, 0), + cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS, + ) + cv2.imshow("Image with Detections", labeled_img) + cv2.waitKey(10) + + +if __name__ == "__main__": + # Parse command-line arguments + parser = argparse.ArgumentParser(description="HSV Color Thresholding test script") + parser.add_argument("--min_h", type=int, default=0) + parser.add_argument("--max_h", type=int, default=255) + parser.add_argument("--min_s", type=int, default=220) + parser.add_argument("--max_s", type=int, default=255) + parser.add_argument("--min_v", type=int, default=0) + parser.add_argument("--max_v", type=int, default=255) + args = parser.parse_args() + + # Start ROS node and subscriber + rclpy.init() + subscriber = ColorThresholdTester(args) + rclpy.spin(subscriber) + subscriber.destroy_node() + rclpy.shutdown()