├── .gitignore ├── .gitmodules ├── .vscode └── c_cpp_properties.json ├── LICENSE ├── README.md ├── drive_tb3 ├── .vscode │ ├── c_cpp_properties.json │ └── settings.json ├── CMakeLists.txt ├── blender │ ├── untitled.blend │ └── untitled.blend1 ├── config │ ├── nav2.rviz │ └── waffle_pi.yaml ├── launch │ ├── p2_a_spawn_tb3.launch.py │ ├── p2_b_multi_tb3.launch.py │ ├── p2_c_drive_sqaure_multi_tb3.launch.py │ ├── p3_a_maths.launch.py │ ├── p5_a_maze_solve.launch.py │ ├── p5_b_line_following.launch.py │ ├── p6_a_maze_mapping.launch.py │ └── p6_b_maze_navigation.launch.py ├── map │ ├── maze_map.pgm │ └── maze_map.yaml ├── models │ ├── line_track │ │ ├── model.config │ │ └── model.sdf │ ├── maze │ │ ├── model.config │ │ └── model.sdf │ └── meshes │ │ ├── base.dae │ │ └── line.dae ├── package.xml ├── src │ ├── p2_multi_tb3_sqaure_drive.cpp │ ├── p3_a_acceleration.cpp │ ├── p3_b_angular_velocity.cpp │ ├── p3_c_linear_goal.cpp │ ├── p3_d_xy_goal.cpp │ ├── p5_a_lidar_data_sub.cpp │ ├── p5_b_maze_solving.cpp │ ├── p5_c_camera_data_sub.cpp │ ├── p5_d_line_following.cpp │ ├── p6_a_occupancy_grid.cpp │ ├── p6_b_sdf_spawner.cpp │ └── p6_c_commander_api_go_to_pose.py └── worlds │ ├── line_following.world │ └── maze.world ├── drive_turtle ├── CMakeLists.txt ├── launch │ ├── p1_a_multi_robot_multi_sim.launch.py │ ├── p1_b_multi_robot_single_sim.launch.py │ ├── p1_c_multi_robot_controller.launch.py │ └── p1_d_run_multi_turtlesim_drive.launch.py ├── package.xml └── src │ ├── p1_a_single_straight_drive.cpp │ ├── p1_b_multi_straight_drive.cpp │ └── p1_c_multi_turtle_controller.cpp ├── microROS_bot ├── .gitignore ├── .vscode │ └── extensions.json ├── Readme.md ├── include │ └── README ├── lib │ ├── README │ ├── motor_control │ │ ├── motor_control.cpp │ │ └── motor_control.h │ ├── oled_display │ │ ├── oled_display.cpp │ │ └── oled_display.h │ └── ros_communication │ │ ├── ros_communication.cpp │ │ └── ros_communication.h ├── platformio.ini ├── src │ └── main.cpp └── test │ └── README └── point_cloud_processing ├── CMakeLists.txt ├── launch └── p9_a_rtab_mapping.launch.py ├── package.xml ├── point_clouds ├── circular_cloud.pcd ├── plane.pcd ├── plane_.pcd ├── table_scene.pcd └── tb3_world.pcd ├── resource ├── .38553d294371dbd5752426cc805039aeae63dd03.cam ├── bun0.pcd └── output.pcd └── src ├── p9_a_table_scene.cpp ├── p9_b_planner_cloud.cpp ├── p9_c_circular_cloud.cpp ├── p9_d_filtering_segmentation.cpp └── p9_e_point_path_drawing.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | .pio 3 | .vscodes 4 | *.d 5 | *.pcd 6 | # Compiled Object files 7 | *.slo 8 | *.lo 9 | *.o 10 | *.obj 11 | 12 | # Precompiled Headers 13 | *.gch 14 | *.pch 15 | 16 | # Compiled Dynamic libraries 17 | *.so 18 | *.dylib 19 | *.dll 20 | 21 | # Fortran module files 22 | *.mod 23 | *.smod 24 | 25 | # Compiled Static libraries 26 | *.lai 27 | *.la 28 | *.a 29 | *.lib 30 | 31 | # Executables 32 | *.exe 33 | *.out 34 | *.app 35 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "turtlebot3_simulations"] 2 | path = turtlebot3_simulations 3 | url = https://github.com/noshluk2/turtlebot3_simulations.git 4 | branch = humble-devel 5 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "${default}", 6 | "limitSymbolsToIncludedHeaders": false 7 | }, 8 | "includePath": [ 9 | "/home/luqman/robotisim_ws/install/turtlebot3_gazebo/include/**", 10 | "/home/luqman/robotisim_ws/install/turtlebot3_fake_node/include/**", 11 | "/home/luqman/ros2_ws/install/turtlebot3_gazebo/include/**", 12 | "/home/luqman/ros2_ws/install/turtlebot3_fake_node/include/**", 13 | "/opt/ros/humble/include/**", 14 | "/home/luqman/ros2_ws/src/ROS2-Point-Cloud-Clustering-and-Segmentation-for-Autonomous-Behaviour main point_cloud_processing/point_cloud_processing/include/**", 15 | "/home/luqman/ros2_ws/src/ROS2-Point-Cloud-Clustering-and-Segmentation-for-Autonomous-Behaviour main point_cloud_processing/turtlebot3_simulations/turtlebot3_gazebo/include/**", 16 | "/home/luqman/ros2_ws/src/drive_tb3/include/**", 17 | "/home/luqman/ros2_ws/src/drive_turtle/include/**", 18 | "/home/luqman/ros2_ws/src/ros2_mobile_robots/drive_tb3/include/**", 19 | "/home/luqman/ros2_ws/src/ros2_mobile_robots/point_cloud_processing/include/**", 20 | "/home/luqman/ros2_ws/src/ros2_mobile_robots/turtlebot3/turtlebot3_node/include/**", 21 | "/home/luqman/ros2_ws/src/ros2_mobile_robots/turtlebot3_simulations/turtlebot3_gazebo/include/**", 22 | "/home/luqman/ros2_ws/src/solving_ros_problems/solve_cpp/include/**", 23 | "/home/luqman/ros2_ws/src/turtlebot3_simulations/turtlebot3_fake_node/include/**", 24 | "/home/luqman/ros2_ws/src/turtlebot3_simulations/turtlebot3_gazebo/include/**", 25 | "/usr/include/**" 26 | ], 27 | "name": "ROS", 28 | "intelliSenseMode": "gcc-x64", 29 | "compilerPath": "/usr/bin/gcc", 30 | "cStandard": "gnu11", 31 | "cppStandard": "c++14" 32 | } 33 | ], 34 | "version": 4 35 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS2 Mobile Robotics 2 | This repository will be carrying code for basics of mobile robotics with ros2 implementation in CPP . 3 | 4 | ## Running Projects 5 | - Project [#1](https://github.com/Robotisim/mobile_robotics_ROS2/issues/1): Multi Turtlesim Driving Simulation -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%231:-Multi-Turtlesim-Driving) 6 | - Project [#2](https://github.com/Robotisim/mobile_robotics_ROS2/issues/2): Patrolling TB3s and gazebo worlds -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%232%3A-Patrolling-TB3s-and-gazebo-worlds) 7 | - Project [#3](https://github.com/Robotisim/mobile_robotics_ROS2/issues/3): Mathematics for mobile Robotics -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%233:-Mathematics-for-mobile-Robotics) 8 | 9 | - Project [#4](https://github.com/Robotisim/mobile_robotics_ROS2/issues/23): MicroROS Controlled Differential Drive Bot -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%234:-MicroROS-Controlled-Differential-Drive-Bot) 10 | 11 | - Project [#5](https://github.com/Robotisim/mobile_robotics_ROS2/issues/5): Line Follower and Maze Solving robots -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%235:-Line-Follower-and-Maze-Solving-robots) 12 | 13 | - Project [#6](https://github.com/Robotisim/mobile_robotics_ROS2/issues/6): Autonomous Turtlebot3 SLAM BOT -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%236:-Autonomous-Turtlebot3-SLAM-BOT) 14 | 15 | - Project [#7](https://github.com/Robotisim/mobile_robotics_ROS2/issues/7): 16 | Autonomous 2D Lidar RPI4 based Robot -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%237:-Autonomous-2D-Lidar-RPI4-based-Robot) 17 | 18 | - Project [#8](https://github.com/Robotisim/mobile_robotics_ROS2/issues/8): Behaviour Trees and State machines -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%238:-Behaviour-Trees-and-State-machines) 19 | 20 | - Project [#9](https://github.com/Robotisim/mobile_robotics_ROS2/issues/9): Rtabmap and Point clouds -> [Wiki_link](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%239:-Rtabmap-and-Point-clouds) 21 | 22 | 23 | ## Requirements 24 | ### Software 25 | - Ubuntu 22.04 26 | - ROS2 Humble 27 | - Vscode 28 | - PlatformIO 29 | 30 | ### Hardware 31 | Get hardware 3D printing body and electronic components from [This repository](https://github.com/Robotisim/mobile_robotics_3D_printing/tree/main) 32 | 33 | -------------------------------------------------------------------------------- /drive_tb3/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "${default}", 6 | "limitSymbolsToIncludedHeaders": false 7 | }, 8 | "includePath": [ 9 | "/home/luqman/robotisim_ws/install/turtlebot3_gazebo/include/**", 10 | "/home/luqman/robotisim_ws/install/turtlebot3_fake_node/include/**", 11 | "/home/luqman/ros2_ws/install/turtlebot3_gazebo/include/**", 12 | "/home/luqman/ros2_ws/install/turtlebot3_fake_node/include/**", 13 | "/opt/ros/humble/include/**", 14 | "/home/luqman/robotisim_ws/src/drive_tb3/include/**", 15 | "/home/luqman/robotisim_ws/src/drive_turltesim/include/**", 16 | "/home/luqman/robotisim_ws/src/turtlebot3/turtlebot3_node/include/**", 17 | "/home/luqman/robotisim_ws/src/turtlebot3_simulations/turtlebot3_fake_node/include/**", 18 | "/home/luqman/robotisim_ws/src/turtlebot3_simulations/turtlebot3_gazebo/include/**", 19 | "/usr/include/**" 20 | ], 21 | "name": "ROS", 22 | "intelliSenseMode": "gcc-x64", 23 | "compilerPath": "/usr/bin/gcc", 24 | "cStandard": "gnu11", 25 | "cppStandard": "c++14" 26 | } 27 | ], 28 | "version": 4 29 | } -------------------------------------------------------------------------------- /drive_tb3/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/luqman/robotisim_ws/install/turtlebot3_teleop/lib/python3.10/site-packages", 4 | "/home/luqman/robotisim_ws/install/turtlebot3_example/lib/python3.10/site-packages", 5 | "/home/luqman/ros2_ws/install/turtlebot3_teleop/lib/python3.10/site-packages", 6 | "/home/luqman/ros2_ws/install/turtlebot3_example/lib/python3.10/site-packages", 7 | "/opt/ros/humble/lib/python3.10/site-packages", 8 | "/opt/ros/humble/local/lib/python3.10/dist-packages" 9 | ], 10 | "python.analysis.extraPaths": [ 11 | "/home/luqman/robotisim_ws/install/turtlebot3_teleop/lib/python3.10/site-packages", 12 | "/home/luqman/robotisim_ws/install/turtlebot3_example/lib/python3.10/site-packages", 13 | "/home/luqman/ros2_ws/install/turtlebot3_teleop/lib/python3.10/site-packages", 14 | "/home/luqman/ros2_ws/install/turtlebot3_example/lib/python3.10/site-packages", 15 | "/opt/ros/humble/lib/python3.10/site-packages", 16 | "/opt/ros/humble/local/lib/python3.10/dist-packages" 17 | ] 18 | } -------------------------------------------------------------------------------- /drive_tb3/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(drive_tb3) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(geometry_msgs REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(tf2 REQUIRED) 14 | find_package(sensor_msgs REQUIRED) 15 | find_package(cv_bridge REQUIRED) 16 | find_package(OpenCV REQUIRED) 17 | find_package(gazebo_msgs REQUIRED) 18 | 19 | 20 | add_executable(p2_multi_tb3_sqaure_drive src/p2_multi_tb3_sqaure_drive.cpp) 21 | ament_target_dependencies(p2_multi_tb3_sqaure_drive rclcpp geometry_msgs) 22 | 23 | add_executable(p3_a_acceleration src/p3_a_acceleration.cpp) 24 | ament_target_dependencies(p3_a_acceleration rclcpp geometry_msgs) 25 | 26 | 27 | add_executable(p3_b_angular_velocity src/p3_b_angular_velocity.cpp) 28 | ament_target_dependencies(p3_b_angular_velocity rclcpp geometry_msgs) 29 | 30 | add_executable(p3_c_linear_goal src/p3_c_linear_goal.cpp) 31 | ament_target_dependencies(p3_c_linear_goal rclcpp geometry_msgs nav_msgs) 32 | 33 | add_executable(p3_d_xy_goal src/p3_d_xy_goal.cpp) 34 | ament_target_dependencies(p3_d_xy_goal rclcpp geometry_msgs nav_msgs tf2) 35 | 36 | add_executable(p5_a_lidar_data_sub src/p5_a_lidar_data_sub.cpp) 37 | ament_target_dependencies(p5_a_lidar_data_sub rclcpp geometry_msgs sensor_msgs) 38 | 39 | add_executable(p5_b_maze_solving src/p5_b_maze_solving.cpp) 40 | ament_target_dependencies(p5_b_maze_solving rclcpp geometry_msgs sensor_msgs) 41 | 42 | add_executable(p5_c_camera_data_sub src/p5_c_camera_data_sub.cpp) 43 | ament_target_dependencies(p5_c_camera_data_sub rclcpp geometry_msgs sensor_msgs cv_bridge) 44 | target_link_libraries(p5_c_camera_data_sub ${OpenCV_LIBS}) 45 | 46 | add_executable(p5_d_line_following src/p5_d_line_following.cpp) 47 | ament_target_dependencies(p5_d_line_following rclcpp geometry_msgs sensor_msgs cv_bridge) 48 | target_link_libraries(p5_d_line_following ${OpenCV_LIBS}) 49 | 50 | add_executable(p6_a_occupancy_grid src/p6_a_occupancy_grid.cpp) 51 | ament_target_dependencies(p6_a_occupancy_grid rclcpp geometry_msgs nav_msgs) 52 | 53 | add_executable(p6_b_sdf_spawner src/p6_b_sdf_spawner.cpp) 54 | ament_target_dependencies(p6_b_sdf_spawner rclcpp gazebo_msgs) 55 | 56 | 57 | install(TARGETS p2_multi_tb3_sqaure_drive 58 | p3_a_acceleration p3_b_angular_velocity p3_c_linear_goal p3_d_xy_goal 59 | p5_a_lidar_data_sub p5_b_maze_solving p5_c_camera_data_sub p5_d_line_following 60 | p6_a_occupancy_grid p6_b_sdf_spawner 61 | 62 | DESTINATION lib/${PROJECT_NAME}) 63 | 64 | 65 | install(PROGRAMS 66 | src/p6_c_commander_api_go_to_pose.py 67 | 68 | DESTINATION lib/${PROJECT_NAME}) 69 | 70 | 71 | 72 | install(DIRECTORY launch worlds models map config 73 | DESTINATION share/${PROJECT_NAME}/ 74 | ) 75 | 76 | 77 | 78 | ament_package() 79 | -------------------------------------------------------------------------------- /drive_tb3/blender/untitled.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotisim/mobile_robotics_ROS2/568f7d790ce31b4664e7774458d1f43ae099fd72/drive_tb3/blender/untitled.blend -------------------------------------------------------------------------------- /drive_tb3/blender/untitled.blend1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotisim/mobile_robotics_ROS2/568f7d790ce31b4664e7774458d1f43ae099fd72/drive_tb3/blender/untitled.blend1 -------------------------------------------------------------------------------- /drive_tb3/config/waffle_pi.yaml: -------------------------------------------------------------------------------- 1 | amcl: 2 | ros__parameters: 3 | use_sim_time: False 4 | alpha1: 0.2 5 | alpha2: 0.2 6 | alpha3: 0.2 7 | alpha4: 0.2 8 | alpha5: 0.2 9 | base_frame_id: "base_footprint" 10 | beam_skip_distance: 0.5 11 | beam_skip_error_threshold: 0.9 12 | beam_skip_threshold: 0.3 13 | do_beamskip: false 14 | global_frame_id: "map" 15 | lambda_short: 0.1 16 | laser_likelihood_max_dist: 2.0 17 | laser_max_range: 100.0 18 | laser_min_range: -1.0 19 | laser_model_type: "likelihood_field" 20 | max_beams: 60 21 | max_particles: 2000 22 | min_particles: 500 23 | odom_frame_id: "odom" 24 | pf_err: 0.05 25 | pf_z: 0.99 26 | recovery_alpha_fast: 0.0 27 | recovery_alpha_slow: 0.0 28 | resample_interval: 1 29 | robot_model_type: "nav2_amcl::DifferentialMotionModel" 30 | save_pose_rate: 0.5 31 | sigma_hit: 0.2 32 | tf_broadcast: true 33 | transform_tolerance: 1.0 34 | update_min_a: 0.2 35 | update_min_d: 0.25 36 | z_hit: 0.5 37 | z_max: 0.05 38 | z_rand: 0.5 39 | z_short: 0.05 40 | 41 | amcl_map_client: 42 | ros__parameters: 43 | use_sim_time: False 44 | 45 | amcl_rclcpp_node: 46 | ros__parameters: 47 | use_sim_time: False 48 | 49 | bt_navigator: 50 | ros__parameters: 51 | use_sim_time: False 52 | global_frame: map 53 | robot_base_frame: base_link 54 | odom_topic: /odom 55 | default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" 56 | bt_loop_duration: 10 57 | default_server_timeout: 20 58 | enable_groot_monitoring: True 59 | groot_zmq_publisher_port: 1666 60 | groot_zmq_server_port: 1667 61 | plugin_lib_names: 62 | - nav2_compute_path_to_pose_action_bt_node 63 | - nav2_compute_path_through_poses_action_bt_node 64 | - nav2_follow_path_action_bt_node 65 | - nav2_back_up_action_bt_node 66 | - nav2_spin_action_bt_node 67 | - nav2_wait_action_bt_node 68 | - nav2_clear_costmap_service_bt_node 69 | - nav2_is_stuck_condition_bt_node 70 | - nav2_goal_reached_condition_bt_node 71 | - nav2_goal_updated_condition_bt_node 72 | - nav2_initial_pose_received_condition_bt_node 73 | - nav2_reinitialize_global_localization_service_bt_node 74 | - nav2_rate_controller_bt_node 75 | - nav2_distance_controller_bt_node 76 | - nav2_speed_controller_bt_node 77 | - nav2_truncate_path_action_bt_node 78 | - nav2_goal_updater_node_bt_node 79 | - nav2_recovery_node_bt_node 80 | - nav2_pipeline_sequence_bt_node 81 | - nav2_round_robin_node_bt_node 82 | - nav2_transform_available_condition_bt_node 83 | - nav2_time_expired_condition_bt_node 84 | - nav2_distance_traveled_condition_bt_node 85 | - nav2_single_trigger_bt_node 86 | - nav2_is_battery_low_condition_bt_node 87 | - nav2_navigate_through_poses_action_bt_node 88 | - nav2_navigate_to_pose_action_bt_node 89 | - nav2_remove_passed_goals_action_bt_node 90 | - nav2_planner_selector_bt_node 91 | - nav2_controller_selector_bt_node 92 | - nav2_goal_checker_selector_bt_node 93 | 94 | bt_navigator_rclcpp_node: 95 | ros__parameters: 96 | use_sim_time: False 97 | 98 | controller_server: 99 | ros__parameters: 100 | use_sim_time: False 101 | controller_frequency: 10.0 102 | min_x_velocity_threshold: 0.001 103 | min_y_velocity_threshold: 0.5 104 | min_theta_velocity_threshold: 0.001 105 | failure_tolerance: 0.3 106 | progress_checker_plugin: "progress_checker" 107 | goal_checker_plugins: ["general_goal_checker"] 108 | controller_plugins: ["FollowPath"] 109 | 110 | # Progress checker parameters 111 | progress_checker: 112 | plugin: "nav2_controller::SimpleProgressChecker" 113 | required_movement_radius: 0.5 114 | movement_time_allowance: 10.0 115 | 116 | general_goal_checker: 117 | stateful: True 118 | plugin: "nav2_controller::SimpleGoalChecker" 119 | xy_goal_tolerance: 0.25 120 | yaw_goal_tolerance: 0.25 121 | 122 | # DWB parameters 123 | FollowPath: 124 | plugin: "dwb_core::DWBLocalPlanner" 125 | debug_trajectory_details: True 126 | min_vel_x: 0.0 127 | min_vel_y: 0.0 128 | max_vel_x: 0.22 129 | max_vel_y: 0.0 130 | max_vel_theta: 1.0 131 | min_speed_xy: 0.0 132 | max_speed_xy: 0.22 133 | min_speed_theta: 0.0 134 | # Add high threshold velocity for turtlebot 3 issue. 135 | # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 136 | acc_lim_x: 2.5 137 | acc_lim_y: 0.0 138 | acc_lim_theta: 3.2 139 | decel_lim_x: -2.5 140 | decel_lim_y: 0.0 141 | decel_lim_theta: -3.2 142 | vx_samples: 20 143 | vy_samples: 0 144 | vtheta_samples: 40 145 | sim_time: 2.0 146 | linear_granularity: 0.05 147 | angular_granularity: 0.025 148 | transform_tolerance: 0.2 149 | xy_goal_tolerance: 0.05 150 | trans_stopped_velocity: 0.25 151 | short_circuit_trajectory_evaluation: True 152 | stateful: True 153 | critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] 154 | BaseObstacle.scale: 0.02 155 | PathAlign.scale: 32.0 156 | PathAlign.forward_point_distance: 0.1 157 | GoalAlign.scale: 24.0 158 | GoalAlign.forward_point_distance: 0.1 159 | PathDist.scale: 32.0 160 | GoalDist.scale: 24.0 161 | RotateToGoal.scale: 32.0 162 | RotateToGoal.slowing_factor: 5.0 163 | RotateToGoal.lookahead_time: -1.0 164 | 165 | controller_server_rclcpp_node: 166 | ros__parameters: 167 | use_sim_time: False 168 | 169 | local_costmap: 170 | local_costmap: 171 | ros__parameters: 172 | update_frequency: 5.0 173 | publish_frequency: 2.0 174 | global_frame: odom 175 | robot_base_frame: base_link 176 | use_sim_time: False 177 | rolling_window: true 178 | width: 3 179 | height: 3 180 | resolution: 0.05 181 | robot_radius: 0.22 182 | plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] 183 | inflation_layer: 184 | plugin: "nav2_costmap_2d::InflationLayer" 185 | inflation_radius: 1.0 186 | cost_scaling_factor: 3.0 187 | obstacle_layer: 188 | plugin: "nav2_costmap_2d::ObstacleLayer" 189 | enabled: True 190 | observation_sources: scan 191 | scan: 192 | topic: /scan 193 | max_obstacle_height: 2.0 194 | clearing: True 195 | marking: True 196 | data_type: "LaserScan" 197 | voxel_layer: 198 | plugin: "nav2_costmap_2d::VoxelLayer" 199 | enabled: True 200 | publish_voxel_map: True 201 | origin_z: 0.0 202 | z_resolution: 0.05 203 | z_voxels: 16 204 | max_obstacle_height: 2.0 205 | mark_threshold: 0 206 | observation_sources: scan 207 | scan: 208 | topic: /scan 209 | max_obstacle_height: 2.0 210 | clearing: True 211 | marking: True 212 | data_type: "LaserScan" 213 | raytrace_max_range: 3.0 214 | raytrace_min_range: 0.0 215 | obstacle_max_range: 2.5 216 | obstacle_min_range: 0.0 217 | static_layer: 218 | map_subscribe_transient_local: True 219 | always_send_full_costmap: True 220 | local_costmap_client: 221 | ros__parameters: 222 | use_sim_time: False 223 | local_costmap_rclcpp_node: 224 | ros__parameters: 225 | use_sim_time: False 226 | 227 | global_costmap: 228 | global_costmap: 229 | ros__parameters: 230 | update_frequency: 1.0 231 | publish_frequency: 1.0 232 | global_frame: map 233 | robot_base_frame: base_link 234 | use_sim_time: True 235 | robot_radius: 0.22 236 | resolution: 0.05 237 | track_unknown_space: true 238 | plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] 239 | obstacle_layer: 240 | plugin: "nav2_costmap_2d::ObstacleLayer" 241 | enabled: True 242 | observation_sources: scan 243 | scan: 244 | topic: /scan 245 | max_obstacle_height: 2.0 246 | clearing: True 247 | marking: True 248 | data_type: "LaserScan" 249 | raytrace_max_range: 3.0 250 | raytrace_min_range: 0.0 251 | obstacle_max_range: 2.5 252 | obstacle_min_range: 0.0 253 | voxel_layer: 254 | plugin: "nav2_costmap_2d::VoxelLayer" 255 | enabled: True 256 | publish_voxel_map: True 257 | origin_z: 0.0 258 | z_resolution: 0.05 259 | z_voxels: 16 260 | max_obstacle_height: 2.0 261 | mark_threshold: 0 262 | observation_sources: scan 263 | scan: 264 | topic: /scan 265 | max_obstacle_height: 2.0 266 | clearing: True 267 | marking: True 268 | data_type: "LaserScan" 269 | raytrace_max_range: 3.0 270 | raytrace_min_range: 0.0 271 | obstacle_max_range: 2.5 272 | obstacle_min_range: 0.0 273 | static_layer: 274 | plugin: "nav2_costmap_2d::StaticLayer" 275 | map_subscribe_transient_local: True 276 | inflation_layer: 277 | plugin: "nav2_costmap_2d::InflationLayer" 278 | cost_scaling_factor: 3.0 279 | inflation_radius: 0.55 280 | always_send_full_costmap: True 281 | global_costmap_client: 282 | ros__parameters: 283 | use_sim_time: False 284 | global_costmap_rclcpp_node: 285 | ros__parameters: 286 | use_sim_time: False 287 | 288 | map_server: 289 | ros__parameters: 290 | use_sim_time: False 291 | yaml_filename: "map.yaml" 292 | 293 | map_saver: 294 | ros__parameters: 295 | use_sim_time: False 296 | save_map_timeout: 5.0 297 | free_thresh_default: 0.25 298 | occupied_thresh_default: 0.65 299 | map_subscribe_transient_local: True 300 | 301 | planner_server: 302 | ros__parameters: 303 | expected_planner_frequency: 20.0 304 | use_sim_time: False 305 | planner_plugins: ["GridBased"] 306 | GridBased: 307 | plugin: "nav2_navfn_planner/NavfnPlanner" 308 | tolerance: 0.5 309 | use_astar: false 310 | allow_unknown: true 311 | 312 | planner_server_rclcpp_node: 313 | ros__parameters: 314 | use_sim_time: False 315 | 316 | recoveries_server: 317 | ros__parameters: 318 | costmap_topic: local_costmap/costmap_raw 319 | footprint_topic: local_costmap/published_footprint 320 | cycle_frequency: 10.0 321 | recovery_plugins: ["spin", "backup", "wait"] 322 | spin: 323 | plugin: "nav2_recoveries/Spin" 324 | backup: 325 | plugin: "nav2_recoveries/BackUp" 326 | wait: 327 | plugin: "nav2_recoveries/Wait" 328 | global_frame: odom 329 | robot_base_frame: base_link 330 | transform_timeout: 0.1 331 | use_sim_time: true 332 | simulate_ahead_time: 2.0 333 | max_rotational_vel: 1.0 334 | min_rotational_vel: 0.4 335 | rotational_acc_lim: 3.2 336 | 337 | robot_state_publisher: 338 | ros__parameters: 339 | use_sim_time: False 340 | 341 | waypoint_follower: 342 | ros__parameters: 343 | loop_rate: 200 344 | stop_on_failure: false 345 | waypoint_task_executor_plugin: "wait_at_waypoint" 346 | wait_at_waypoint: 347 | plugin: "nav2_waypoint_follower::WaitAtWaypoint" 348 | enabled: True 349 | waypoint_pause_duration: 200 350 | -------------------------------------------------------------------------------- /drive_tb3/launch/p2_a_spawn_tb3.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file initiates a Gazebo simulation, spawning a TurtleBot3 model at specified coordinates 6 | using gazebo_ros and turtlebot3_gazebo packages. 7 | 8 | Packages used: 9 | 10 | - gazebo_ros 11 | - turtlebot3_gazebo 12 | 13 | Arguments used: 14 | 15 | - x_pos: The x-coordinate where the TurtleBot3 model should be spawned in the Gazebo world. 16 | - y_pos: The y-coordinate where the TurtleBot3 model should be spawned in the Gazebo world. 17 | - yaw_rot: The yaw rotation of the TurtleBot3 model when it is spawned. 18 | - robot_name: The name of the TurtleBot3 model to be spawned. 19 | - robot_ns: The namespace into which the TurtleBot3 model should be spawned. 20 | """ 21 | 22 | #!/usr/bin/env python3 23 | 24 | import os 25 | 26 | from ament_index_python.packages import get_package_share_directory 27 | from launch import LaunchDescription 28 | from launch.actions import IncludeLaunchDescription 29 | from launch.launch_description_sources import PythonLaunchDescriptionSource 30 | from launch.substitutions import LaunchConfiguration 31 | from launch_ros.actions import Node 32 | 33 | 34 | def generate_launch_description(): 35 | ##### File Paths are stored 36 | # This section retrieves the path of the TurtleBot3 model based on the environment variable 'TURTLEBOT3_MODEL'. 37 | pkg_gazebo_ros = get_package_share_directory('gazebo_ros') 38 | TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL'] 39 | model_folder = 'turtlebot3_' + TURTLEBOT3_MODEL 40 | urdf_path = os.path.join( 41 | get_package_share_directory('turtlebot3_gazebo'), 42 | 'models', 43 | model_folder, 44 | 'model.sdf' 45 | ) 46 | 47 | ##### Define arguments - parameters 48 | # These lines define the parameters for the TurtleBot3 model's spawn location and its namespace. 49 | x_pos = LaunchConfiguration('x_pos',default='0.0') 50 | y_pos = LaunchConfiguration('y_pos',default='0.0') 51 | yaw_rot = LaunchConfiguration('yaw_rot',default='0.0') 52 | robot_name = LaunchConfiguration('robot_name',default='waffle_pi') 53 | robot_ns = LaunchConfiguration('robot_ns',default='') 54 | 55 | ##### Spawn the TurtleBot3 model at the specified location 56 | # This section spawns the TurtleBot3 model at the specified location in the Gazebo simulation. 57 | start_gazebo_ros_spawner_cmd = Node( 58 | package='gazebo_ros', 59 | executable='spawn_entity.py', 60 | arguments=[ 61 | '-entity', robot_name, 62 | '-file', urdf_path, 63 | '-x', x_pos, 64 | '-y', y_pos, 65 | '-z', '0.01', 66 | '-Y',yaw_rot, 67 | '-robot_namespace',robot_ns 68 | ], 69 | output='screen', 70 | ) 71 | 72 | ld = LaunchDescription() 73 | 74 | ##### Add the commands to the launch description 75 | # This line adds the spawn command to the launch description. 76 | ld.add_action(start_gazebo_ros_spawner_cmd) 77 | 78 | # Return the LaunchDescription 79 | return ld 80 | -------------------------------------------------------------------------------- /drive_tb3/launch/p2_b_multi_tb3.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file initiates a Gazebo simulation in a specified world and spawns four TurtleBot3 models at distinct locations. 6 | 7 | Arguments used: 8 | - world: Full path to the Gazebo world file to load. 9 | - x_pos: The x-coordinate where the TurtleBot3 model should be spawned in the Gazebo world. 10 | - y_pos: The y-coordinate where the TurtleBot3 model should be spawned in the Gazebo world. 11 | - yaw_rot: The yaw rotation of the TurtleBot3 model when it is spawned. 12 | - robot_name: The name of the TurtleBot3 model to be spawned. 13 | - robot_ns: The namespace into which the TurtleBot3 model should be spawned. 14 | 15 | Packages used: 16 | - gazebo_ros: This package provides nodes and plugins necessary to run, interface and interact with Gazebo simulations from ROS2. In this launch file, it is used to start the Gazebo server and client. 17 | - turtlebot3_gazebo: This package contains the Gazebo simulation environments for TurtleBot3. In this launch file, it is used to retrieve the path of the Gazebo world file. 18 | - drive_tb3: This is a custom package that is likely to contain launch files and nodes necessary to control the TurtleBot3 models. In this launch file, it is used to spawn the TurtleBot3 models at specified locations in the Gazebo simulation. 19 | 20 | 21 | 22 | 23 | 24 | """ 25 | 26 | #!/usr/bin/env python3 27 | import os 28 | 29 | from ament_index_python.packages import get_package_share_directory 30 | from launch import LaunchDescription 31 | from launch.actions import IncludeLaunchDescription 32 | from launch.launch_description_sources import PythonLaunchDescriptionSource 33 | from launch_ros.actions import Node 34 | 35 | 36 | def generate_launch_description(): 37 | ##### File Paths are stored 38 | # This section retrieves the path of the gazebo_ros and drive_tb3 packages. 39 | pkg_gazebo_ros = get_package_share_directory('gazebo_ros') 40 | pkg_tb3_drive = get_package_share_directory('drive_tb3') 41 | 42 | #### Gazebo Launching and importing 43 | # This section retrieves the path to the desired Gazebo world file. 44 | world = os.path.join( 45 | get_package_share_directory('turtlebot3_gazebo'), 46 | 'worlds', 47 | 'turtlebot3_dqn_stage1.world' 48 | ) 49 | 50 | ##### Start the Gazebo server with the specified world 51 | # This section starts the Gazebo server with the specified world. 52 | gzserver_cmd = IncludeLaunchDescription( 53 | PythonLaunchDescriptionSource( 54 | os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') 55 | ), 56 | launch_arguments={'world': world}.items() 57 | ) 58 | 59 | ##### Start the Gazebo client 60 | # This section starts the Gazebo client. 61 | gzclient_cmd = IncludeLaunchDescription( 62 | PythonLaunchDescriptionSource( 63 | os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') 64 | ) 65 | ) 66 | 67 | ##### Robot Spawning 68 | # These sections spawn four TurtleBot3 models at specified locations using the p2_a_spawn_tb3.launch.py launch file from the drive_tb3 package. 69 | robot_spawner1 = IncludeLaunchDescription( 70 | PythonLaunchDescriptionSource( 71 | os.path.join(pkg_tb3_drive, 'launch', 'p2_a_spawn_tb3.launch.py') 72 | ), 73 | launch_arguments={ 74 | 'x_pos' :'2.0' , 75 | 'y_pos' :'-2.0', 76 | 'yaw_rot' :'1.57', 77 | 'robot_name' :'tb_1', 78 | 'robot_ns' : 'robot_a' 79 | }.items() 80 | ) 81 | 82 | robot_spawner2 = IncludeLaunchDescription( 83 | PythonLaunchDescriptionSource( 84 | os.path.join(pkg_tb3_drive, 'launch', 'p2_a_spawn_tb3.launch.py') 85 | ), 86 | launch_arguments={ 87 | 'x_pos' :'2.0' , 88 | 'y_pos' :'2.0', 89 | 'yaw_rot' :'3.14', 90 | 'robot_name' :'tb_2', 91 | 'robot_ns' : 'robot_b' 92 | }.items() 93 | ) 94 | 95 | robot_spawner3 = IncludeLaunchDescription( 96 | PythonLaunchDescriptionSource( 97 | os.path.join(pkg_tb3_drive, 'launch', 'p2_a_spawn_tb3.launch.py') 98 | ), 99 | launch_arguments={ 100 | 'x_pos' :'-2.0' , 101 | 'y_pos' :'2.0', 102 | 'yaw_rot' :'-1.57', 103 | 'robot_name' :'tb_3', 104 | 'robot_ns' : 'robot_c' 105 | }.items() 106 | ) 107 | 108 | robot_spawner4 = IncludeLaunchDescription( 109 | PythonLaunchDescriptionSource( 110 | os.path.join(pkg_tb3_drive, 'launch', 'p2_a_spawn_tb3.launch.py') 111 | ), 112 | launch_arguments={ 113 | 'x_pos' :'-2.0' , 114 | 'y_pos' :'-2.0', 115 | 'yaw_rot' :'0.0', 116 | 'robot_name' :'tb_4', 117 | 'robot_ns' : 'robot_d' 118 | }.items() 119 | ) 120 | 121 | ld = LaunchDescription() 122 | 123 | ##### Add the commands to the launch description 124 | # This section adds all the IncludeLaunchDescription actions to the launch description. 125 | ld.add_action(gzserver_cmd) 126 | ld.add_action(gzclient_cmd) 127 | ld.add_action(robot_spawner1) 128 | ld.add_action(robot_spawner2) 129 | ld.add_action(robot_spawner3) 130 | ld.add_action(robot_spawner4) 131 | 132 | # Return the LaunchDescription 133 | return ld 134 | -------------------------------------------------------------------------------- /drive_tb3/launch/p2_c_drive_sqaure_multi_tb3.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file includes another launch file to spawn multiple TurtleBot3 models and starts four nodes each driving a 6 | distinct TurtleBot3 model. 7 | 8 | Arguments used: 9 | - world: Full path to the Gazebo world file to load. 10 | - x_pos: The x-coordinate where the TurtleBot3 model should be spawned in the Gazebo world. 11 | - y_pos: The y-coordinate where the TurtleBot3 model should be spawned in the Gazebo world. 12 | - yaw_rot: The yaw rotation of the TurtleBot3 model when it is spawned. 13 | - robot_name: The name of the TurtleBot3 model to be spawned. 14 | - robot_ns: The namespace into which the TurtleBot3 model should be spawned. 15 | 16 | Packages used: 17 | - gazebo_ros: This package provides nodes and plugins necessary to run, interface and interact with Gazebo simulations from ROS2. In this launch file, it is used to start the Gazebo server and client. 18 | - turtlebot3_gazebo: This package contains the Gazebo simulation environments for TurtleBot3. In this launch file, it is used to retrieve the path of the Gazebo world file. 19 | - drive_tb3: This is a custom package that is likely to contain launch files and nodes necessary to control the TurtleBot3 models. In this launch file, it is used to spawn the TurtleBot3 models at specified locations in the Gazebo simulation. 20 | 21 | """ 22 | 23 | #!/usr/bin/env python3 24 | import os 25 | 26 | from ament_index_python.packages import get_package_share_directory 27 | from launch import LaunchDescription 28 | from launch.actions import IncludeLaunchDescription 29 | from launch.launch_description_sources import PythonLaunchDescriptionSource 30 | from launch_ros.actions import Node 31 | 32 | 33 | def generate_launch_description(): 34 | ##### File Paths are stored 35 | # This section retrieves the path of the drive_tb3 package. 36 | pkg_tb3_drive = get_package_share_directory('drive_tb3') 37 | 38 | ##### Include the launch file to spawn multiple TurtleBot3 models 39 | # This section includes the p2_b_multi_tb3.launch.py launch file from the drive_tb3 package to spawn multiple TurtleBot3 models. 40 | spawn_multi_tb3 = IncludeLaunchDescription( 41 | PythonLaunchDescriptionSource( 42 | os.path.join(pkg_tb3_drive, 'launch', 'p2_b_multi_tb3.launch.py') 43 | ), 44 | ) 45 | 46 | ##### Start the node to drive the first TurtleBot3 model 47 | # These sections start four nodes to drive each of the TurtleBot3 models. The cmd_vel_topic parameter is set to the command velocity topic of each robot. 48 | drive_tb3_a = Node( 49 | package='drive_tb3', 50 | executable='p2_multi_tb3_sqaure_drive', 51 | name='drive_tb3_a', 52 | parameters=[ 53 | {'cmd_vel_topic': '/robot_a/cmd_vel'}, 54 | ] 55 | ) 56 | 57 | drive_tb3_b = Node( 58 | package='drive_tb3', 59 | executable='p2_multi_tb3_sqaure_drive', 60 | name='drive_tb3_b', 61 | parameters=[ 62 | {'cmd_vel_topic': '/robot_b/cmd_vel'}, 63 | ] 64 | ) 65 | 66 | drive_tb3_c = Node( 67 | package='drive_tb3', 68 | executable='p2_multi_tb3_sqaure_drive', 69 | name='drive_tb3_c', 70 | parameters=[ 71 | {'cmd_vel_topic': '/robot_c/cmd_vel'}, 72 | ] 73 | ) 74 | 75 | drive_tb3_d = Node( 76 | package='drive_tb3', 77 | executable='p2_multi_tb3_sqaure_drive', 78 | name='drive_tb3_d', 79 | parameters=[ 80 | {'cmd_vel_topic': '/robot_d/cmd_vel'}, 81 | ] 82 | ) 83 | 84 | ld = LaunchDescription() 85 | 86 | ##### Add the commands to the launch description 87 | # This section adds all the IncludeLaunchDescription and Node actions to the launch description. 88 | ld.add_action(spawn_multi_tb3) 89 | ld.add_action(drive_tb3_a) 90 | ld.add_action(drive_tb3_b) 91 | ld.add_action(drive_tb3_c) 92 | ld.add_action(drive_tb3_d) 93 | 94 | # Return the LaunchDescription 95 | return ld 96 | -------------------------------------------------------------------------------- /drive_tb3/launch/p3_a_maths.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file includes the 'empty_world.launch.py' file to spawn a TurtleBot3 model in an empty world, and starts two nodes: 6 | 1. The 'rqt_reconfigure' node, which provides a GUI for dynamically reconfiguring parameters. 7 | 2. The 'p3_d_xy_goal' node from the 'drive_tb3' package, which drives the TurtleBot3 model. 8 | 9 | 10 | 11 | Packages used: 12 | - turtlebot3_gazebo: This package is used to spawn a TurtleBot3 model in an empty world. The launch file empty_world.launch.py is specifically included from this package. 13 | - rqt_reconfigure: This package provides a node that is used to set up a GUI for dynamic reconfiguration of parameters. 14 | - drive_tb3: This is a custom package likely containing nodes for controlling the TurtleBot3. The p3_d_xy_goal node from this package is used to drive the TurtleBot3 model. 15 | """ 16 | 17 | #!/usr/bin/env python3 18 | import os 19 | 20 | from ament_index_python.packages import get_package_share_directory 21 | from launch import LaunchDescription 22 | from launch.actions import IncludeLaunchDescription 23 | from launch.launch_description_sources import PythonLaunchDescriptionSource 24 | from launch_ros.actions import Node 25 | 26 | 27 | def generate_launch_description(): 28 | ##### File Paths are stored 29 | # This section retrieves the path of the turtlebot3_gazebo package. 30 | pkg_tb3_drive = get_package_share_directory('turtlebot3_gazebo') 31 | 32 | ##### Include the launch file to spawn TurtleBot3 model in an empty world 33 | # This section includes the empty_world.launch.py launch file from the turtlebot3_gazebo package to spawn a TurtleBot3 model in an empty world. 34 | spawn_tb3 = IncludeLaunchDescription( 35 | PythonLaunchDescriptionSource( 36 | os.path.join(pkg_tb3_drive, 'launch', 'empty_world.launch.py') 37 | ), 38 | ) 39 | 40 | ##### Start the rqt_reconfigure node 41 | # This section starts the rqt_reconfigure node, which provides a GUI for dynamically reconfiguring parameters. 42 | reconfigure = Node( 43 | package='rqt_reconfigure', 44 | executable='rqt_reconfigure', 45 | name='parameter_reconfig' 46 | ) 47 | 48 | ##### Start the node to drive the TurtleBot3 model 49 | # This section starts the p3_d_xy_goal node from the drive_tb3 package to drive the TurtleBot3 model. 50 | drive_tb3_b = Node( 51 | package='drive_tb3', 52 | executable='p3_d_xy_goal', 53 | name='custom_node' 54 | ) 55 | 56 | ld = LaunchDescription() 57 | 58 | ##### Add the commands to the launch description 59 | # This section adds all the IncludeLaunchDescription and Node actions to the launch description. 60 | ld.add_action(spawn_tb3) 61 | ld.add_action(reconfigure) 62 | ld.add_action(drive_tb3_b) 63 | 64 | # Return the LaunchDescription 65 | return ld 66 | -------------------------------------------------------------------------------- /drive_tb3/launch/p5_a_maze_solve.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file starts a Gazebo simulation with a specified maze world, spawns a TurtleBot3 model at a specified location 6 | and starts a node for maze solving. 7 | 8 | 9 | Arguments used: 10 | - 'world': Path to the Gazebo world file to load 11 | - 'x_pos': x-coordinate position to spawn the TurtleBot3 model 12 | - 'y_pos': y-coordinate position to spawn the TurtleBot3 model 13 | - 'yaw_rot': yaw rotation to spawn the TurtleBot3 model 14 | - 'robot_name': Name of the TurtleBot3 model 15 | - 'robot_ns': Namespace of the TurtleBot3 model 16 | 17 | Packages used: 18 | - drive_tb3: This custom package is used to specify the maze world file for Gazebo, spawn the TurtleBot3 model at a specific location, and solve the maze using the p5_b_maze_solving node. 19 | - gazebo_ros: This package is used to launch Gazebo with the specified world file. The launch files gzserver.launch.py and gzclient.launch.py are included from this package. 20 | """ 21 | 22 | #!/usr/bin/env python3 23 | import os 24 | 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import IncludeLaunchDescription 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | from launch_ros.actions import Node 30 | 31 | 32 | def generate_launch_description(): 33 | ############# Paths Setting 34 | # This section retrieves the paths of the drive_tb3 and gazebo_ros packages and sets the path of the world file. 35 | pkg_tb3_drive = get_package_share_directory('drive_tb3') 36 | pkg_gazebo_ros = get_package_share_directory('gazebo_ros') 37 | world = os.path.join( 38 | pkg_tb3_drive, 39 | 'worlds', 40 | 'maze.world' 41 | ) 42 | 43 | ############# Gazebo Definitions 44 | # These sections start the Gazebo server and client with the specified world. 45 | gzserver_cmd = IncludeLaunchDescription( 46 | PythonLaunchDescriptionSource( 47 | os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') 48 | ), 49 | launch_arguments={'world': world}.items() 50 | ) 51 | 52 | gzclient_cmd = IncludeLaunchDescription( 53 | PythonLaunchDescriptionSource( 54 | os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') 55 | ) 56 | ) 57 | 58 | ############# Spawning Robot 59 | # This section spawns a TurtleBot3 model at a specified location using the p2_a_spawn_tb3.launch.py launch file from the drive_tb3 package. 60 | robot_spawner = IncludeLaunchDescription( 61 | PythonLaunchDescriptionSource( 62 | os.path.join(pkg_tb3_drive, 'launch', 'p2_a_spawn_tb3.launch.py') 63 | ), 64 | launch_arguments={ 65 | 'x_pos': '-7.9', 66 | 'y_pos': '-8.76', 67 | 'yaw_rot': '1.57', 68 | 'robot_name' :'turtlebot_3', 69 | 'robot_ns' : 'maze_solver' 70 | 71 | }.items() 72 | ) 73 | 74 | ############# Lidar Processing Node 75 | # This section starts the p5_b_maze_solving node from the drive_tb3 package for maze solving. 76 | maze_solve_tb3 = Node( 77 | package='drive_tb3', 78 | executable='p5_b_maze_solving', 79 | name='lidar_processing' 80 | ) 81 | 82 | ld = LaunchDescription() 83 | 84 | # Add the commands to the launch description 85 | ld.add_action(gzserver_cmd) 86 | ld.add_action(gzclient_cmd) 87 | ld.add_action(robot_spawner) 88 | ld.add_action(maze_solve_tb3) 89 | 90 | # Return the LaunchDescription 91 | return ld 92 | -------------------------------------------------------------------------------- /drive_tb3/launch/p5_b_line_following.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file starts a Gazebo simulation with a specified world, spawns a line track model and a TurtleBot3 model 6 | at specified locations, and starts a node for line following. 7 | 8 | Arguments used: 9 | - 'world': Path to the Gazebo world file to load 10 | - 'x_pos': x-coordinate position to spawn the TurtleBot3 model 11 | - 'y_pos': y-coordinate position to spawn the TurtleBot3 model 12 | - 'yaw_rot': yaw rotation to spawn the TurtleBot3 model 13 | - 'robot_name': Name of the TurtleBot3 model 14 | - 'maze_path': Path to the maze model file (in SDF format) 15 | 16 | Packages used: 17 | - drive_tb3: This custom package is used to specify the line following world file for Gazebo, spawn the TurtleBot3 model and line track model at specific locations, and start the line following node (p5_d_line_following). 18 | - gazebo_ros: This package is used to launch Gazebo with the specified world file. The launch files gzserver.launch.py and gzclient.launch.py are included from this package. 19 | 20 | """ 21 | 22 | #!/usr/bin/env python3 23 | import os 24 | 25 | from ament_index_python.packages import get_package_share_directory 26 | from launch import LaunchDescription 27 | from launch.actions import IncludeLaunchDescription 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | from launch_ros.actions import Node 30 | 31 | 32 | def generate_launch_description(): 33 | ############# Paths Setting 34 | # This section retrieves the paths of the drive_tb3 and gazebo_ros packages, sets the path of the line track model and the world file. 35 | pkg_tb3_drive = get_package_share_directory('drive_tb3') 36 | pkg_gazebo_ros = get_package_share_directory('gazebo_ros') 37 | maze_path = os.path.join(pkg_tb3_drive,'models','line_track','model.sdf') 38 | 39 | world = os.path.join( 40 | pkg_tb3_drive, 41 | 'worlds', 42 | 'line_following.world' 43 | ) 44 | 45 | ############# Gazebo Definitions 46 | # These sections start the Gazebo server and client with the specified world. 47 | gzserver_cmd = IncludeLaunchDescription( 48 | PythonLaunchDescriptionSource( 49 | os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') 50 | ), 51 | launch_arguments={'world': world}.items() 52 | ) 53 | 54 | gzclient_cmd = IncludeLaunchDescription( 55 | PythonLaunchDescriptionSource( 56 | os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') 57 | ) 58 | ) 59 | 60 | ############# Spawning Robot 61 | # This section spawns a TurtleBot3 model at a specified location using the p2_a_spawn_tb3.launch.py launch file from the drive_tb3 package. 62 | robot_spawner = IncludeLaunchDescription( 63 | PythonLaunchDescriptionSource( 64 | os.path.join(pkg_tb3_drive, 'launch', 'p2_a_spawn_tb3.launch.py') 65 | ), 66 | launch_arguments={ 67 | 'x_pos': '2.16', 68 | 'y_pos': '-1.04', 69 | 'yaw_rot': '3.11', 70 | 'robot_name' :'turtlebot_3', 71 | }.items() 72 | ) 73 | 74 | ############# Lidar Processing Node 75 | # This section starts the p5_d_line_following node from the drive_tb3 package for line following. 76 | follow_line_tb3 = Node( 77 | package='drive_tb3', 78 | executable='p5_d_line_following', 79 | name='Line_following' 80 | ) 81 | 82 | ############# Spawning Maze Model 83 | # This section starts the p6_b_sdf_spawner node from the drive_tb3 package to spawn the line track model. 84 | maze_spawner = Node( 85 | package='drive_tb3', 86 | executable='p6_b_sdf_spawner', 87 | name='maze_model', 88 | arguments=[maze_path,"Maze","0.0","0.0"] 89 | ) 90 | 91 | ld = LaunchDescription() 92 | 93 | # Add the commands to the launch description 94 | ld.add_action(gzserver_cmd) 95 | ld.add_action(gzclient_cmd) 96 | ld.add_action(maze_spawner) 97 | ld.add_action(robot_spawner) 98 | ld.add_action(follow_line_tb3) 99 | 100 | # Return the LaunchDescription 101 | return ld 102 | -------------------------------------------------------------------------------- /drive_tb3/launch/p6_a_maze_mapping.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file starts a Gazebo simulation, spawns a TurtleBot3 model and a maze, 6 | then starts the mapping functionality and Rviz2 for visualization. 7 | 8 | 9 | Arguments used: 10 | 11 | - 'use_sim_time': Use simulation (Gazebo) clock if true 12 | - 'x_pos': x-coordinate position to spawn the TurtleBot3 model 13 | - 'y_pos': y-coordinate position to spawn the TurtleBot3 model 14 | - 'yaw_rot': yaw rotation to spawn the TurtleBot3 model 15 | - 'robot_name': Name of the TurtleBot3 model 16 | - 'maze_path': Path to the maze model file (in SDF format) 17 | - 'rviz_config': Path to the Rviz configuration file 18 | 19 | Packages used: 20 | - turtlebot3_gazebo: This package is used to specify the directory for the launch files and to start the robot_state_publisher node. 21 | - drive_tb3: This custom package is used to spawn the TurtleBot3 model and the maze. 22 | - gazebo_ros: This package is used to launch Gazebo. The launch files gzserver.launch.py and gzclient.launch.py are included from this package. 23 | - slam_toolbox: This package is used to start the mapping functionality via the online_async_launch.py launch file. 24 | - rviz2: This package is used to start the Rviz2 node for visualization. 25 | 26 | 27 | 28 | 29 | 30 | """ 31 | 32 | #!/usr/bin/env python3 33 | import os 34 | 35 | from ament_index_python.packages import get_package_share_directory 36 | from launch import LaunchDescription 37 | from launch.actions import IncludeLaunchDescription 38 | from launch.launch_description_sources import PythonLaunchDescriptionSource 39 | from launch_ros.actions import Node 40 | 41 | 42 | def generate_launch_description(): 43 | ############# Paths Setting 44 | # This section sets the paths of the launch file directory, maze model, and Rviz configuration file. 45 | launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') 46 | pkg_tb3_drive = get_package_share_directory('drive_tb3') 47 | pkg_gazebo_ros = get_package_share_directory('gazebo_ros') 48 | maze_path = os.path.join(pkg_tb3_drive,'models','maze','model.sdf') 49 | config_dir = os.path.join(get_package_share_directory('turtlebot3_navigation2'),'config') 50 | rviz_config= os.path.join(config_dir,'tb3_navigation2.rviz') 51 | 52 | ############# Gazebo Definitions 53 | # These sections start the Gazebo server and client. 54 | gzserver_cmd = IncludeLaunchDescription( 55 | PythonLaunchDescriptionSource( 56 | os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') 57 | ), 58 | ) 59 | gzclient_cmd = IncludeLaunchDescription( 60 | PythonLaunchDescriptionSource( 61 | os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') 62 | ) 63 | ) 64 | 65 | ############# Spawning Robot and Maze 66 | # This section starts the robot_state_publisher node, spawns a TurtleBot3 model at a specified location, and spawns a maze. 67 | robot_state_publisher_cmd = IncludeLaunchDescription( 68 | PythonLaunchDescriptionSource( 69 | os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') 70 | ), 71 | launch_arguments={'use_sim_time': 'True'}.items() 72 | ) 73 | robot_spawner = IncludeLaunchDescription( 74 | PythonLaunchDescriptionSource( 75 | os.path.join(pkg_tb3_drive, 'launch', 'p2_a_spawn_tb3.launch.py') 76 | ), 77 | launch_arguments={ 78 | 'x_pos': '-6.1', 79 | 'y_pos': '-7.1', 80 | 'yaw_rot': '1.57', 81 | 'robot_name' :'turtlebot_3', 82 | }.items() 83 | ) 84 | maze_spawner = Node( 85 | package='drive_tb3', 86 | executable='p6_b_sdf_spawner', 87 | name='maze_model', 88 | arguments=[maze_path,"Maze","0.0","0.0"] 89 | ) 90 | 91 | ############# Mapping Functionality 92 | # This section starts the online_async_launch.py launch file from the slam_toolbox package for mapping. 93 | maze_mapping = IncludeLaunchDescription( 94 | PythonLaunchDescriptionSource( 95 | os.path.join(get_package_share_directory('slam_toolbox'),'launch', 'online_async_launch.py') 96 | ), 97 | ) 98 | 99 | ############ Rviz Configuration 100 | # This section starts the Rviz2 node for visualization. 101 | rviz = Node( 102 | package='rviz2', 103 | executable='rviz2', 104 | name='rviz2', 105 | arguments=['-d', rviz_config], 106 | output='screen' 107 | ) 108 | 109 | # Add 2 more Nodes for spawning robot with navigation package 110 | 111 | ld = LaunchDescription() 112 | 113 | # Add the commands to the launch description 114 | ld.add_action(gzserver_cmd) 115 | ld.add_action(gzclient_cmd) 116 | ld.add_action(robot_spawner) 117 | ld.add_action(maze_spawner) 118 | ld.add_action(robot_state_publisher_cmd) 119 | ld.add_action(maze_mapping) 120 | ld.add_action(rviz) 121 | 122 | # Return the LaunchDescription 123 | return ld 124 | -------------------------------------------------------------------------------- /drive_tb3/launch/p6_b_maze_navigation.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Darby Lim 3 | Modified by: Muhammad Luqman 4 | Organization: Robotisim 5 | 6 | This launch file starts a Gazebo simulation, spawns a TurtleBot3 model and a maze, and then starts the navigation2 functionality along with Rviz2 for visualization. 7 | 8 | Packages used: 9 | - gazebo_ros 10 | - drive_tb3 11 | - turtlebot3_gazebo 12 | - nav2_bringup 13 | - rviz2 14 | 15 | Arguments used: 16 | - map: Full path to map file to load 17 | - params_file: Full path to param file to load 18 | - use_sim_time: Use simulation (Gazebo) clock if true 19 | """ 20 | 21 | #!/usr/bin/env python3 22 | import os 23 | 24 | from ament_index_python.packages import get_package_share_directory 25 | from launch import LaunchDescription 26 | from launch.actions import DeclareLaunchArgument 27 | from launch.actions import IncludeLaunchDescription 28 | from launch.launch_description_sources import PythonLaunchDescriptionSource 29 | from launch.substitutions import LaunchConfiguration 30 | from launch_ros.actions import Node 31 | 32 | TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL'] 33 | 34 | 35 | def generate_launch_description(): 36 | ############# Paths Setting 37 | # This section sets the paths of the launch file directory, maze model, Rviz configuration file, and navigation2 bringup. 38 | pkg_gazebo_ros = get_package_share_directory('gazebo_ros') 39 | pkg_tb3_drive = get_package_share_directory('drive_tb3') 40 | maze_path = os.path.join(pkg_tb3_drive,'models','maze','model.sdf') 41 | use_sim_time = LaunchConfiguration('use_sim_time', default='false') 42 | launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') 43 | 44 | map_dir = LaunchConfiguration( 45 | 'map', 46 | default=os.path.join( 47 | get_package_share_directory('drive_tb3'), 48 | 'map', 49 | 'maze_map.yaml')) 50 | 51 | param_file_name = TURTLEBOT3_MODEL + '.yaml' 52 | param_dir = LaunchConfiguration( 53 | 'params_file', 54 | default=os.path.join( 55 | get_package_share_directory('drive_tb3'), 56 | 'config', 57 | param_file_name)) 58 | 59 | nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') 60 | 61 | rviz_config_dir = os.path.join( 62 | get_package_share_directory('drive_tb3'), 63 | 'config', 64 | 'nav2.rviz') 65 | 66 | return LaunchDescription([ 67 | # Arguments 68 | # This section declares the launch arguments. 69 | DeclareLaunchArgument( 70 | 'map', 71 | default_value=map_dir, 72 | description='Full path to map file to load'), 73 | 74 | DeclareLaunchArgument( 75 | 'params_file', 76 | default_value=param_dir, 77 | description='Full path to param file to load'), 78 | 79 | DeclareLaunchArgument( 80 | 'use_sim_time', 81 | default_value='false', 82 | description='Use simulation (Gazebo) clock if true'), 83 | 84 | # Launch Files included 85 | # These sections start the Gazebo server and client and the robot_state_publisher node. 86 | IncludeLaunchDescription( 87 | PythonLaunchDescriptionSource( 88 | os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py') 89 | ), 90 | ), 91 | 92 | IncludeLaunchDescription( 93 | PythonLaunchDescriptionSource( 94 | os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py') 95 | ) 96 | ), 97 | 98 | IncludeLaunchDescription( 99 | PythonLaunchDescriptionSource( 100 | os.path.join(launch_file_dir, 'robot_state_publisher.launch.py') 101 | ), 102 | launch_arguments={'use_sim_time': 'True'}.items() 103 | ), 104 | 105 | # Spawning Robot 106 | # This section spawns a TurtleBot3 model at a specified location. 107 | IncludeLaunchDescription( 108 | PythonLaunchDescriptionSource( 109 | os.path.join(pkg_tb3_drive, 'launch', 'p2_a_spawn_tb3.launch.py') 110 | ), 111 | launch_arguments={ 112 | 'x_pos': '-6.1', 113 | 'y_pos': '-7.1', 114 | 'yaw_rot': '1.57', 115 | 'robot_name' :'turtlebot_3', 116 | }.items() 117 | ), 118 | 119 | # Integerating Nav2 Stack 120 | # This section starts the bringup_launch.py launch file from the nav2_bringup package for navigation2 functionality. 121 | IncludeLaunchDescription( 122 | PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']), 123 | launch_arguments={ 124 | 'map': map_dir, 125 | 'use_sim_time': use_sim_time, 126 | 'params_file': param_dir}.items(), 127 | ), 128 | 129 | # Spawn Maze 130 | # This section spawns a maze. 131 | Node( 132 | package='drive_tb3', 133 | executable='p6_b_sdf_spawner', 134 | name='maze_model', 135 | arguments=[maze_path,"Maze","0.0","0.0"] 136 | ), 137 | 138 | # Launch Rviz2 with Configuration 139 | # This section starts the Rviz2 node for visualization. 140 | Node( 141 | package='rviz2', 142 | executable='rviz2', 143 | name='rviz2', 144 | arguments=['-d', rviz_config_dir], 145 | parameters=[{'use_sim_time': use_sim_time}], 146 | output='screen'), 147 | ]) 148 | -------------------------------------------------------------------------------- /drive_tb3/map/maze_map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotisim/mobile_robotics_ROS2/568f7d790ce31b4664e7774458d1f43ae099fd72/drive_tb3/map/maze_map.pgm -------------------------------------------------------------------------------- /drive_tb3/map/maze_map.yaml: -------------------------------------------------------------------------------- 1 | image: maze_map.pgm 2 | mode: trinary 3 | resolution: 0.05 4 | origin: [-8.84, -7.42, 0] 5 | negate: 0 6 | occupied_thresh: 0.65 7 | free_thresh: 0.25 -------------------------------------------------------------------------------- /drive_tb3/models/line_track/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | line_track 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /drive_tb3/models/line_track/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1 7 | 8 | 0.166667 9 | 0 10 | 0 11 | 0.166667 12 | 0 13 | 0.166667 14 | 15 | 16 | 0.848639 -0.049037 0 0 -0 0 17 | 18 | 0 0 0 0 -0 0 19 | 20 | 21 | /home/luqman/robotisim_ws/src/drive_tb3/models/meshes/base.dae 22 | 1 1 1 23 | 24 | 25 | 26 | 1 27 | 31 | 32 | 0 0 0 1 33 | 34 | 0 35 | 1 36 | 37 | 38 | 0 39 | 10 40 | 0 0 0 0 -0 0 41 | 42 | 43 | /home/luqman/robotisim_ws/src/drive_tb3/models/meshes/base.dae 44 | 1 1 1 45 | 46 | 47 | 48 | 49 | 50 | 1 51 | 1 52 | 0 0 0 53 | 0 54 | 0 55 | 56 | 57 | 1 58 | 0 59 | 0 60 | 1 61 | 62 | 0 63 | 64 | 65 | 66 | 67 | 0 68 | 1e+06 69 | 70 | 71 | 0 72 | 1 73 | 1 74 | 75 | 0 76 | 0.2 77 | 1e+13 78 | 1 79 | 0.01 80 | 0 81 | 82 | 83 | 1 84 | -0.01 85 | 0 86 | 0.2 87 | 1e+13 88 | 1 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 1 97 | 98 | 0.166667 99 | 0 100 | 0 101 | 0.166667 102 | 0 103 | 0.166667 104 | 105 | 106 | -0.848635 0.049036 0.0001 0 -0 0 107 | 108 | 0 0 -0.03 0 -0 0 109 | 110 | 111 | /home/luqman/robotisim_ws/src/drive_tb3/models/meshes/line.dae 112 | 1 1 1 113 | 114 | 115 | 116 | 1 117 | 121 | 122 | 0 0 0 1 123 | 124 | 0 125 | 1 126 | 127 | 128 | 0 129 | 10 130 | 0 0 -0.03 0 -0 0 131 | 132 | 133 | /home/luqman/robotisim_ws/src/drive_tb3/models/meshes/line.dae 134 | 1 1 1 135 | 136 | 137 | 138 | 139 | 140 | 1 141 | 1 142 | 0 0 0 143 | 0 144 | 0 145 | 146 | 147 | 1 148 | 0 149 | 0 150 | 1 151 | 152 | 0 153 | 154 | 155 | 156 | 157 | 0 158 | 1e+06 159 | 160 | 161 | 0 162 | 1 163 | 1 164 | 165 | 0 166 | 0.2 167 | 1e+13 168 | 1 169 | 0.01 170 | 0 171 | 172 | 173 | 1 174 | -0.01 175 | 0 176 | 0.2 177 | 1e+13 178 | 1 179 | 180 | 181 | 182 | 183 | 184 | 185 | link_0 186 | link_1 187 | 0 0 0 0 -0 0 188 | 189 | 190 | 191 | 0 192 | 0.2 193 | 194 | 195 | 0 196 | 0.2 197 | 198 | 199 | 200 | 201 | 1 202 | 1 203 | 204 | 205 | -------------------------------------------------------------------------------- /drive_tb3/models/maze/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | maze 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /drive_tb3/models/maze/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -0.106834 0.387097 0 0 -0 0 5 | 6 | 7 | 8 | 9 | 13.5 0.15 2.5 10 | 11 | 12 | 0 0 1.25 0 -0 0 13 | 14 | 15 | 0 0 1.25 0 -0 0 16 | 17 | 18 | 13.5 0.15 2.5 19 | 20 | 21 | 22 | 26 | 1 1 1 1 27 | 28 | 29 | 0 30 | 31 | 32 | 7.95747 -1.05965 0 0 -0 -1.5708 33 | 34 | 35 | 36 | 37 | 38 | 12.5 0.15 2.5 39 | 40 | 41 | 0 0 1.25 0 -0 0 42 | 43 | 44 | 0 0 1.25 0 -0 0 45 | 46 | 47 | 12.5 0.15 2.5 48 | 49 | 50 | 51 | 55 | 1 1 1 1 56 | 57 | 58 | 0 59 | 60 | 61 | 1.78247 -7.73465 0 0 -0 3.14159 62 | 63 | 64 | 65 | 66 | 67 | 11.75 0.15 2.5 68 | 69 | 70 | 0 0 1.25 0 -0 0 71 | 72 | 73 | 0 0 1.25 0 -0 0 74 | 75 | 76 | 11.75 0.15 2.5 77 | 78 | 79 | 80 | 84 | 1 1 1 1 85 | 86 | 87 | 0 88 | 89 | 90 | 0.014797 5.03689 0 0 -0 0 91 | 92 | 93 | 94 | 95 | 96 | 12 0.15 2.5 97 | 98 | 99 | 0 0 1.25 0 -0 0 100 | 101 | 102 | 0 0 1.25 0 -0 0 103 | 104 | 105 | 12 0.15 2.5 106 | 107 | 108 | 109 | 113 | 1 1 1 1 114 | 115 | 116 | 0 117 | 118 | 119 | -2.03247 -4.37626 0 0 -0 0 120 | 121 | 122 | 123 | 124 | 125 | 7 0.15 2.5 126 | 127 | 128 | 0 0 1.25 0 -0 0 129 | 130 | 131 | 0 0 1.25 0 -0 0 132 | 133 | 134 | 7 0.15 2.5 135 | 136 | 137 | 138 | 142 | 1 1 1 1 143 | 144 | 145 | 0 146 | 147 | 148 | 4.50725 -2.00286 0 0 -0 3.14159 149 | 150 | 151 | 152 | 153 | 154 | 4.75 0.15 2.5 155 | 156 | 157 | 0 0 1.25 0 -0 0 158 | 159 | 160 | 0 0 1.25 0 -0 0 161 | 162 | 163 | 4.75 0.15 2.5 164 | 165 | 166 | 167 | 171 | 1 1 1 1 172 | 173 | 174 | 0 175 | 176 | 177 | 1.08225 0.297139 0 0 -0 1.5708 178 | 179 | 180 | 181 | 182 | 183 | 7.25 0.15 2.5 184 | 185 | 186 | 0 0 1.25 0 -0 0 187 | 188 | 189 | 0 0 1.25 0 -0 0 190 | 191 | 192 | 7.25 0.15 2.5 193 | 194 | 195 | 196 | 200 | 1 1 1 1 201 | 202 | 203 | 0 204 | 205 | 206 | -4.98066 1.52712 0 0 -0 -1.5708 207 | 208 | 209 | 210 | 211 | 212 | 5.25 0.15 2.5 213 | 214 | 215 | 0 0 1.25 0 -0 0 216 | 217 | 218 | 0 0 1.25 0 -0 0 219 | 220 | 221 | 5.25 0.15 2.5 222 | 223 | 224 | 225 | 229 | 1 1 1 1 230 | 231 | 232 | 0 233 | 234 | 235 | 4.03022 2.48689 0 0 -0 -1.5708 236 | 237 | 238 | 239 | 240 | 241 | 15.5 0.15 2.5 242 | 243 | 244 | 0 0 1.25 0 -0 0 245 | 246 | 247 | 0 0 1.25 0 -0 0 248 | 249 | 250 | 15.5 0.15 2.5 251 | 252 | 253 | 254 | 258 | 1 1 1 1 259 | 260 | 261 | 0 262 | 263 | 264 | -7.91013 0.059646 0 0 -0 1.5708 265 | 266 | 267 | 268 | 269 | 270 | 16 0.15 2.5 271 | 272 | 273 | 0 0 1.25 0 -0 0 274 | 275 | 276 | 0 0 1.25 0 -0 0 277 | 278 | 279 | 16 0.15 2.5 280 | 281 | 282 | 283 | 287 | 1 1 1 1 288 | 289 | 290 | 0 291 | 292 | 293 | 0.014872 7.73465 0 0 -0 0 294 | 295 | 1 296 | 297 | 298 | -------------------------------------------------------------------------------- /drive_tb3/models/meshes/base.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Blender User 6 | Blender 3.0.1 7 | 8 | 2023-05-22T19:08:28 9 | 2023-05-22T19:08:28 10 | 11 | Z_UP 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 0 0 0 1 20 | 21 | 22 | 0.8 0.8 0.8 1 23 | 24 | 25 | 1.45 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 1 1 1 1 1 -1 1 -1 1 1 -1 -1 -1 1 1 -1 1 -1 -1 -1 1 -1 -1 -1 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 0 0 1 0 -1 0 -1 0 0 0 0 -1 1 0 0 0 1 0 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 0.875 0.5 0.625 0.75 0.625 0.5 0.625 0.75 0.375 1 0.375 0.75 0.625 0 0.375 0.25 0.375 0 0.375 0.5 0.125 0.75 0.125 0.5 0.625 0.5 0.375 0.75 0.375 0.5 0.625 0.25 0.375 0.5 0.375 0.25 0.875 0.5 0.875 0.75 0.625 0.75 0.625 0.75 0.625 1 0.375 1 0.625 0 0.625 0.25 0.375 0.25 0.375 0.5 0.375 0.75 0.125 0.75 0.625 0.5 0.625 0.75 0.375 0.75 0.625 0.25 0.625 0.5 0.375 0.5 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 |

4 0 0 2 0 1 0 0 2 2 1 3 7 1 4 3 1 5 6 2 6 5 2 7 7 2 8 1 3 9 7 3 10 5 3 11 0 4 12 3 4 13 1 4 14 4 5 15 1 5 16 5 5 17 4 0 18 6 0 19 2 0 20 2 1 21 6 1 22 7 1 23 6 2 24 4 2 25 5 2 26 1 3 27 3 3 28 7 3 29 0 4 30 2 4 31 3 4 32 4 5 33 0 5 34 1 5 35

78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 5 0 0 0 0 5 0 0 0 0 0.03 0 0 0 0 1 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /drive_tb3/models/meshes/line.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Blender User 6 | Blender 3.0.1 7 | 8 | 2023-05-22T20:50:17 9 | 2023-05-22T20:50:17 10 | 11 | Z_UP 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -19.1818 -1.012113 0 -17.18181 -1.01061 0 -19.1818 0.9878867 0 -17.18181 0.9893895 0 -17.18181 -1.01061 0 -15.18181 -1.009108 0 -17.18181 0.9893895 0 -15.18181 0.9908923 0 -15.18181 -1.009108 0 -13.18181 -1.007605 0 -15.18181 0.9908923 0 -13.18181 0.9923951 0 -13.18181 -1.007605 0 -11.18181 -1.006102 0 -13.18181 0.9923951 0 -11.18181 0.9938979 0 -11.18181 -1.006102 0 -9.181813 -1.004599 0 -11.18181 0.9938979 0 -9.181813 0.9954007 0 -9.181813 -1.004599 0 -7.181812 -1.003097 0 -9.181813 0.9954007 0 -7.181812 0.9969034 0 -7.181812 -1.003097 0 -5.181812 -1.001594 0 -7.181812 0.9969034 0 -5.181812 0.9984062 0 -5.181812 -1.001594 0 -3.181808 -1.000091 0 -5.181812 0.9984062 0 -3.181808 0.999909 0 -3.181808 -1.000091 0 -1.181725 -0.998588 0 -3.181808 0.999909 0 -1.181898 1.001411 0 -1.181725 -0.998588 0 0.8184795 -0.989288 0 -1.181898 1.001411 0 0.8178978 1.010709 0 0.8184795 -0.989288 0 2.818827 -0.9634313 0 0.8178978 1.010709 0 2.817539 1.036557 0 2.818827 -0.9634313 0 4.819292 -0.9120648 0 2.817539 1.036557 0 4.817027 1.087896 0 4.819292 -0.9120648 0 6.819801 -0.8257384 0 4.817027 1.087896 0 6.816397 1.174174 0 6.819801 -0.8257384 0 8.820231 -0.7039934 0 6.816397 1.174174 0 8.81572 1.295853 0 8.820231 -0.7039934 0 10.82049 -0.5514508 0 8.81572 1.295853 0 10.81507 1.448327 0 10.82049 -0.5514508 0 12.82054 -0.375689 0 10.81507 1.448327 0 12.81451 1.624036 0 12.82054 -0.375689 0 14.82038 -0.1865201 0 12.81451 1.624036 0 14.81408 1.81318 0 14.82038 -0.1865201 0 16.82003 0.00534892 0 14.81408 1.81318 0 16.81382 2.005057 0 16.82003 0.00534892 0 18.81953 0.189189 0 16.81382 2.005057 0 18.81376 2.188937 0 18.81953 0.189189 0 20.81892 0.3546068 0 18.81376 2.188937 0 20.81392 2.354418 0 20.81892 0.3546068 0 22.8182 0.4914888 0 20.81392 2.354418 0 22.81432 2.491375 0 22.8182 0.4914888 0 24.81737 0.5896204 0 22.81432 2.491375 0 24.81497 2.589577 0 24.81737 0.5896204 0 26.81641 0.6380187 0 24.81497 2.589577 0 26.81589 2.638017 0 26.81641 0.6380187 0 28.81522 0.6239981 0 26.81589 2.638017 0 28.81705 2.623973 0 28.81522 0.6239981 0 30.81369 0.5322718 0 28.81705 2.623973 0 30.81842 2.532104 0 30.81369 0.5322718 0 32.81162 0.3446611 0 30.81842 2.532104 0 32.81987 2.344146 0 32.81162 0.3446611 0 34.80871 0.03969901 0 32.81987 2.344146 0 34.82122 2.038515 0 34.80871 0.03969901 0 36.80444 -0.4088923 0 34.82122 2.038515 0 36.82213 1.588738 0 36.80444 -0.4088923 0 38.79798 -1.034101 0 36.82213 1.588738 0 38.82207 0.9615092 0 38.79798 -1.034101 0 40.788 -1.877996 0 38.82207 0.9615092 0 40.82009 0.1142011 0 40.788 -1.877996 0 42.77235 -2.994706 0 40.82009 0.1142011 0 42.81472 -1.008329 0 42.77235 -2.994706 0 44.74756 -4.454124 0 42.81472 -1.008329 0 44.80348 -2.477916 0 44.74756 -4.454124 0 46.7072 -6.365284 0 44.80348 -2.477916 0 46.78172 -4.407734 0 46.7072 -6.365284 0 48.63334 -8.999268 0 46.78172 -4.407734 0 48.7363 -7.081108 0 48.63334 -8.999268 0 50.49106 -12.66771 0 48.7363 -7.081108 0 50.63984 -10.84276 0 50.49106 -12.66771 0 52.16978 -18.11018 0 50.63984 -10.84276 0 52.39625 -16.54545 0 52.16978 -18.11018 0 53.39937 -26.11634 0 52.39625 -16.54545 0 53.72418 -25.21714 0 53.39937 -26.11634 0 53.8174 -36.33728 0 53.72418 -25.21714 0 54.18086 -36.27339 0 53.8174 -36.33728 0 53.80036 -47.23938 0 54.18086 -36.27339 0 54.16328 -47.36513 0 53.80036 -47.23938 0 53.43436 -57.71758 0 54.16328 -47.36513 0 53.77536 -58.41231 0 53.43436 -57.71758 0 52.36991 -66.26148 0 53.77536 -58.41231 0 52.60402 -67.79187 0 52.36991 -66.26148 0 50.66865 -71.11977 0 52.60402 -67.79187 0 50.78044 -73.02291 0 50.66865 -71.11977 0 48.74728 -73.45909 0 50.78044 -73.02291 0 48.80091 -75.43722 0 48.74728 -73.45909 0 46.77157 -74.61451 0 48.80091 -75.43722 0 46.79993 -76.60842 0 46.77157 -74.61451 0 44.78125 -75.25183 0 46.79993 -76.60842 0 44.79727 -77.24988 0 44.78125 -75.25183 0 42.78567 -75.62887 0 44.79727 -77.24988 0 42.79526 -77.62818 0 42.78567 -75.62887 0 40.78775 -75.85449 0 42.79526 -77.62818 0 40.79404 -77.85419 0 40.78775 -75.85449 0 38.78889 -76.01143 0 40.79404 -77.85419 0 38.7933 -78.01129 0 38.78889 -76.01143 0 36.78948 -76.12741 0 38.7933 -78.01129 0 36.79294 -78.12731 0 36.78948 -76.12741 0 34.78971 -76.22454 0 36.79294 -78.12731 0 34.79288 -78.22446 0 34.78971 -76.22454 0 32.78973 -76.31949 0 34.79288 -78.22446 0 32.79299 -78.3194 0 32.78973 -76.31949 0 30.78962 -76.42237 0 32.79299 -78.3194 0 30.79328 -78.42227 0 30.78962 -76.42237 0 28.7894 -76.54144 0 30.79328 -78.42227 0 28.79372 -78.54129 0 28.7894 -76.54144 0 26.78914 -76.68382 0 28.79372 -78.54129 0 26.79434 -78.68361 0 26.78914 -76.68382 0 24.78883 -76.8563 0 26.79434 -78.68361 0 24.79515 -78.856 0 24.78883 -76.8563 0 22.78851 -77.06588 0 24.79515 -78.856 0 22.79619 -79.06544 0 22.78851 -77.06588 0 20.78824 -77.32049 0 22.79619 -79.06544 0 20.79757 -79.31983 0 20.78824 -77.32049 0 18.78803 -77.62972 0 20.79757 -79.31983 0 18.79936 -79.62875 0 18.78803 -77.62972 0 16.78799 -78.00578 0 18.79936 -79.62875 0 16.80176 -80.00434 0 16.78799 -78.00578 0 14.78823 -78.46433 0 16.80176 -80.00434 0 14.80504 -80.46219 0 14.78823 -78.46433 0 12.78894 -79.02595 0 14.80504 -80.46219 0 12.80958 -81.02273 0 12.78894 -79.02595 0 10.79045 -79.71811 0 12.80958 -81.02273 0 10.81605 -81.71315 0 10.79045 -79.71811 0 8.793303 -80.57796 0 10.81605 -81.71315 0 8.825495 -82.5701 0 8.793303 -80.57796 0 6.798563 -81.66617 0 8.825495 -82.5701 0 6.839897 -83.65321 0 6.798563 -81.66617 0 4.809234 -83.12584 0 6.839897 -83.65321 0 4.864705 -85.10243 0 4.809234 -83.12584 0 2.830554 -85.10116 0 4.864705 -85.10243 0 2.908745 -87.05438 0 2.830554 -85.10116 0 0.8821392 -88.04676 0 2.908745 -87.05438 0 1.00326 -89.93256 0 0.8821392 -88.04676 0 -0.9536247 -92.97484 0 1.00326 -89.93256 0 -0.7430296 -94.6053 0 -0.9536247 -92.97484 0 -2.296703 -101.6296 0 -0.7430296 -94.6053 0 -1.966488 -102.4671 0 -2.296703 -101.6296 0 -2.683712 -112.7821 0 -1.966488 -102.4671 0 -2.320158 -112.7401 0 -2.683712 -112.7821 0 -2.282648 -123.8531 0 -2.320158 -112.7401 0 -1.943317 -123.1341 0 -2.282648 -123.8531 0 -1.234657 -133.5539 0 -1.943317 -123.1341 0 -0.9583283 -132.2539 0 -1.234657 -133.5539 0 0.3223658 -140.8438 0 -0.9583283 -132.2539 0 0.5186997 -139.1603 0 0.3223658 -140.8438 0 2.14729 -145.7606 0 0.5186997 -139.1603 0 2.276496 -143.8912 0 2.14729 -145.7606 0 4.081627 -148.992 0 2.276496 -143.8912 0 4.16593 -147.0465 0 4.081627 -148.992 0 6.058949 -151.0595 0 4.16593 -147.0465 0 6.117071 -149.0852 0 6.058949 -151.0595 0 8.049458 -152.5194 0 6.117071 -149.0852 0 8.090752 -150.5324 0 8.049458 -152.5194 0 10.04553 -153.5909 0 8.090752 -150.5324 0 10.07552 -151.5977 0 10.04553 -153.5909 0 12.04444 -154.3483 0 10.07552 -151.5977 0 12.06714 -152.3522 0 12.04444 -154.3483 0 14.04427 -154.9165 0 12.06714 -152.3522 0 14.06179 -152.9188 0 14.04427 -154.9165 0 16.04444 -155.4039 0 14.06179 -152.9188 0 16.05765 -153.4052 0 16.04444 -155.4039 0 18.04493 -155.7153 0 16.05765 -153.4052 0 18.05557 -153.7162 0 18.04493 -155.7153 0 20.04541 -156.0207 0 18.05557 -153.7162 0 20.05352 -154.0212 0 20.04541 -156.0207 0 22.04587 -156.2041 0 20.05352 -154.0212 0 22.05249 -154.2044 0 22.04587 -156.2041 0 24.04635 -156.3874 0 22.05249 -154.2044 0 24.05149 -154.3876 0 24.04635 -156.3874 0 26.04671 -156.5139 0 24.05149 -154.3876 0 26.05082 -154.514 0 26.04671 -156.5139 0 28.04705 -156.6119 0 26.05082 -154.514 0 28.05034 -154.612 0 28.04705 -156.6119 0 30.04737 -156.7099 0 28.05034 -154.612 0 30.04985 -154.7099 0 30.04737 -156.7099 0 32.04757 -156.7624 0 30.04985 -154.7099 0 32.04957 -154.7624 0 32.04757 -156.7624 0 34.04777 -156.807 0 32.04957 -154.7624 0 34.04936 -154.807 0 34.04777 -156.807 0 36.04796 -156.8515 0 34.04936 -154.807 0 36.04912 -154.8515 0 36.04796 -156.8515 0 38.0481 -156.8798 0 36.04912 -154.8515 0 38.04898 -154.8798 0 38.0481 -156.8798 0 40.04819 -156.8943 0 38.04898 -154.8798 0 40.04888 -154.8943 0 40.04819 -156.8943 0 42.04827 -156.9089 0 40.04888 -154.8943 0 42.04878 -154.9089 0 42.04827 -156.9089 0 44.04837 -156.9235 0 42.04878 -154.9089 0 44.0487 -154.9235 0 44.04837 -156.9235 0 46.04841 -156.9294 0 44.0487 -154.9235 0 46.04864 -154.9294 0 46.04841 -156.9294 0 48.04845 -156.9313 0 46.04864 -154.9294 0 48.04862 -154.9313 0 48.04845 -156.9313 0 50.04848 -156.9331 0 48.04862 -154.9313 0 50.04858 -154.9331 0 50.04848 -156.9331 0 52.04851 -156.935 0 50.04858 -154.9331 0 52.04857 -154.935 0 52.04851 -156.935 0 54.04853 -156.9368 0 52.04857 -154.935 0 54.04853 -154.9368 0 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 0 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 |

1 0 0 2 0 1 0 0 2 5 0 3 6 0 4 4 0 5 9 0 6 10 0 7 8 0 8 13 0 9 14 0 10 12 0 11 17 0 12 18 0 13 16 0 14 21 0 15 22 0 16 20 0 17 25 0 18 26 0 19 24 0 20 29 0 21 30 0 22 28 0 23 33 0 24 34 0 25 32 0 26 37 1 27 38 1 28 36 1 29 41 2 30 42 2 31 40 2 32 45 3 33 46 3 34 44 3 35 49 4 36 50 4 37 48 4 38 53 5 39 54 5 40 52 5 41 57 0 42 58 0 43 56 0 44 61 6 45 62 6 46 60 6 47 65 0 48 66 0 49 64 0 50 69 7 51 70 7 52 68 7 53 73 8 54 74 8 55 72 8 56 77 9 57 78 9 58 76 9 59 81 10 60 82 10 61 80 10 62 85 11 63 86 11 64 84 11 65 89 0 66 90 0 67 88 0 68 92 12 69 95 12 70 94 12 71 96 0 72 99 0 73 98 0 74 100 13 75 103 13 76 102 13 77 104 14 78 107 14 79 106 14 80 108 15 81 111 15 82 110 15 83 112 16 84 115 16 85 114 16 86 116 17 87 119 17 88 118 17 89 120 18 90 123 18 91 122 18 92 124 0 93 127 0 94 126 0 95 128 19 96 131 19 97 130 19 98 132 20 99 135 20 100 134 20 101 136 21 102 139 21 103 138 21 104 140 22 105 143 22 106 142 22 107 144 23 108 147 23 109 146 23 110 148 24 111 151 24 112 150 24 113 153 25 114 154 25 115 152 25 116 157 26 117 158 26 118 156 26 119 161 27 120 162 27 121 160 27 122 165 28 123 166 28 124 164 28 125 169 29 126 170 29 127 168 29 128 173 30 129 174 30 130 172 30 131 177 31 132 178 31 133 176 31 134 181 32 135 182 32 136 180 32 137 185 0 138 186 0 139 184 0 140 189 0 141 190 0 142 188 0 143 193 33 144 194 33 145 192 33 146 197 34 147 198 34 148 196 34 149 201 35 150 202 35 151 200 35 152 205 0 153 206 0 154 204 0 155 209 36 156 210 36 157 208 36 158 213 37 159 214 37 160 212 37 161 217 0 162 218 0 163 216 0 164 221 38 165 222 38 166 220 38 167 225 39 168 226 39 169 224 39 170 229 40 171 230 40 172 228 40 173 233 41 174 234 41 175 232 41 176 237 42 177 238 42 178 236 42 179 241 43 180 242 43 181 240 43 182 245 44 183 246 44 184 244 44 185 249 45 186 250 45 187 248 45 188 253 46 189 254 46 190 252 46 191 257 47 192 258 47 193 256 47 194 261 48 195 262 48 196 260 48 197 265 49 198 266 49 199 264 49 200 269 50 201 270 50 202 268 50 203 273 51 204 274 51 205 272 51 206 277 52 207 278 52 208 276 52 209 280 53 210 283 53 211 282 53 212 284 0 213 287 0 214 286 0 215 288 54 216 291 54 217 290 54 218 292 55 219 295 55 220 294 55 221 296 56 222 299 56 223 298 56 224 300 0 225 303 0 226 302 0 227 304 0 228 307 0 229 306 0 230 308 57 231 311 57 232 310 57 233 312 58 234 315 58 235 314 58 236 316 59 237 319 59 238 318 59 239 320 60 240 323 60 241 322 60 242 324 61 243 327 61 244 326 61 245 328 0 246 331 0 247 330 0 248 332 62 249 335 62 250 334 62 251 336 63 252 339 63 253 338 63 254 340 64 255 343 64 256 342 64 257 344 65 258 347 65 259 346 65 260 348 66 261 351 66 262 350 66 263 352 67 264 355 67 265 354 67 266 356 68 267 359 68 268 358 68 269 360 69 270 363 69 271 362 69 272 364 0 273 367 0 274 366 0 275 368 0 276 371 0 277 370 0 278 372 70 279 375 70 280 374 70 281 376 71 282 379 71 283 378 71 284 380 0 285 383 0 286 382 0 287 384 72 288 387 72 289 386 72 290 388 73 291 391 73 292 390 73 293 392 74 294 395 74 295 394 74 296 396 0 297 399 0 298 398 0 299 1 75 300 3 75 301 2 75 302 5 0 303 7 0 304 6 0 305 9 76 306 11 76 307 10 76 308 13 0 309 15 0 310 14 0 311 17 77 312 19 77 313 18 77 314 21 0 315 23 0 316 22 0 317 25 78 318 27 78 319 26 78 320 29 0 321 31 0 322 30 0 323 33 79 324 35 79 325 34 79 326 37 80 327 39 80 328 38 80 329 41 81 330 43 81 331 42 81 332 45 82 333 47 82 334 46 82 335 49 83 336 51 83 337 50 83 338 53 84 339 55 84 340 54 84 341 57 85 342 59 85 343 58 85 344 61 86 345 63 86 346 62 86 347 65 87 348 67 87 349 66 87 350 69 0 351 71 0 352 70 0 353 73 88 354 75 88 355 74 88 356 77 89 357 79 89 358 78 89 359 81 90 360 83 90 361 82 90 362 85 0 363 87 0 364 86 0 365 89 0 366 91 0 367 90 0 368 92 91 369 93 91 370 95 91 371 96 0 372 97 0 373 99 0 374 100 92 375 101 92 376 103 92 377 104 93 378 105 93 379 107 93 380 108 0 381 109 0 382 111 0 383 112 94 384 113 94 385 115 94 386 116 95 387 117 95 388 119 95 389 120 96 390 121 96 391 123 96 392 124 0 393 125 0 394 127 0 395 128 97 396 129 97 397 131 97 398 132 98 399 133 98 400 135 98 401 136 99 402 137 99 403 139 99 404 140 100 405 141 100 406 143 100 407 144 0 408 145 0 409 147 0 410 148 101 411 149 101 412 151 101 413 153 102 414 155 102 415 154 102 416 157 103 417 159 103 418 158 103 419 161 104 420 163 104 421 162 104 422 165 105 423 167 105 424 166 105 425 169 106 426 171 106 427 170 106 428 173 107 429 175 107 430 174 107 431 177 108 432 179 108 433 178 108 434 181 109 435 183 109 436 182 109 437 185 0 438 187 0 439 186 0 440 189 110 441 191 110 442 190 110 443 193 111 444 195 111 445 194 111 446 197 112 447 199 112 448 198 112 449 201 113 450 203 113 451 202 113 452 205 114 453 207 114 454 206 114 455 209 115 456 211 115 457 210 115 458 213 116 459 215 116 460 214 116 461 217 117 462 219 117 463 218 117 464 221 118 465 223 118 466 222 118 467 225 119 468 227 119 469 226 119 470 229 120 471 231 120 472 230 120 473 233 121 474 235 121 475 234 121 476 237 122 477 239 122 478 238 122 479 241 123 480 243 123 481 242 123 482 245 124 483 247 124 484 246 124 485 249 125 486 251 125 487 250 125 488 253 0 489 255 0 490 254 0 491 257 126 492 259 126 493 258 126 494 261 127 495 263 127 496 262 127 497 265 128 498 267 128 499 266 128 500 269 129 501 271 129 502 270 129 503 273 130 504 275 130 505 274 130 506 277 131 507 279 131 508 278 131 509 280 132 510 281 132 511 283 132 512 284 0 513 285 0 514 287 0 515 288 133 516 289 133 517 291 133 518 292 134 519 293 134 520 295 134 521 296 135 522 297 135 523 299 135 524 300 0 525 301 0 526 303 0 527 304 136 528 305 136 529 307 136 530 308 137 531 309 137 532 311 137 533 312 138 534 313 138 535 315 138 536 316 139 537 317 139 538 319 139 539 320 140 540 321 140 541 323 140 542 324 141 543 325 141 544 327 141 545 328 0 546 329 0 547 331 0 548 332 142 549 333 142 550 335 142 551 336 143 552 337 143 553 339 143 554 340 144 555 341 144 556 343 144 557 344 145 558 345 145 559 347 145 560 348 146 561 349 146 562 351 146 563 352 147 564 353 147 565 355 147 566 356 148 567 357 148 568 359 148 569 360 149 570 361 149 571 363 149 572 364 0 573 365 0 574 367 0 575 368 0 576 369 0 577 371 0 578 372 150 579 373 150 580 375 150 581 376 151 582 377 151 583 379 151 584 380 152 585 381 152 586 383 152 587 384 153 588 385 153 589 387 153 590 388 154 591 389 154 592 391 154 593 392 155 594 393 155 595 395 155 596 396 0 597 397 0 598 399 0 599

55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 0.11 0 0 0 0 0.02 0 0 0 0 1 0.06 0 0 0 1 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /drive_tb3/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | drive_tb3 5 | 0.0.0 6 | TODO: Package description 7 | luqman 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclpy 13 | nav2_msgs 14 | 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /drive_tb3/src/p2_multi_tb3_sqaure_drive.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Author: Muhammad Luqman 3 | * Organization: Robotisim 4 | * 5 | * This ROS2 node, "turtlesim_driving", makes a Turtlebot3 robot move in a square pattern. 6 | * 7 | * The node publishes geometry_msgs/Twist messages to a specified command velocity topic 8 | * to control the robot's motion. The robot moves straight for a certain duration, then turns 9 | * for a certain duration, and repeats these steps to complete the square pattern. 10 | */ 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "geometry_msgs/msg/twist.hpp" 18 | 19 | using namespace std::chrono_literals; 20 | 21 | class drive_turtlesim : public rclcpp::Node 22 | { 23 | public: 24 | drive_turtlesim() 25 | : Node("turtlesim_driving"), count_(0) 26 | { 27 | this->declare_parameter("cmd_vel_topic","/turtle1/cmd_vel"); 28 | std::string cmd_vel_topic_var = this->get_parameter("cmd_vel_topic").as_string(); 29 | 30 | 31 | publisher_ = this->create_publisher(cmd_vel_topic_var, 10); 32 | timer_ = this->create_wall_timer( 33 | 500ms, std::bind(&drive_turtlesim::timer_callback, this)); 34 | } 35 | 36 | private: 37 | double linear_velocity = 0.5; 38 | double angular_velocity = 1.2; 39 | double side_length = 2.5; 40 | 41 | void timer_callback() 42 | { 43 | 44 | // Calculate travel time and turn time 45 | double travel_time = side_length / linear_velocity; 46 | double turn_time = 2 + (M_PI / (2.0 * angular_velocity) ); 47 | 48 | // Create the Twist message 49 | auto message = geometry_msgs::msg::Twist(); 50 | 51 | // Move robot straight 52 | message.linear.x = linear_velocity; 53 | message.angular.z = 0.0; 54 | publisher_->publish(message); 55 | rclcpp::sleep_for(std::chrono::duration_cast(std::chrono::duration(travel_time))); 56 | 57 | // Stop robot and make a turn 58 | message.angular.z = angular_velocity; 59 | message.linear.x = angular_velocity; 60 | publisher_->publish(message); 61 | rclcpp::sleep_for(std::chrono::duration_cast(std::chrono::duration(turn_time))); 62 | 63 | } 64 | rclcpp::TimerBase::SharedPtr timer_; 65 | rclcpp::Publisher::SharedPtr publisher_; 66 | size_t count_; 67 | }; 68 | 69 | int main(int argc, char * argv[]) 70 | { 71 | rclcpp::init(argc, argv); 72 | rclcpp::spin(std::make_shared()); 73 | rclcpp::shutdown(); 74 | return 0; 75 | } -------------------------------------------------------------------------------- /drive_tb3/src/p3_a_acceleration.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p3_a_acceleration.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node calculates and outputs the acceleration of a robot and publishes Twist messages for robot motion. 5 | * @organization Robotisim 6 | */ 7 | #include "rclcpp/rclcpp.hpp" 8 | #include "geometry_msgs/msg/twist.hpp" 9 | 10 | class Acceleration_node : public rclcpp::Node 11 | { 12 | public: 13 | Acceleration_node() 14 | : Node("acceleration_cal_node"), last_velocity_(0.0), last_time_(this->now()) 15 | { 16 | this->declare_parameter("linear_velocity", 2.0); 17 | 18 | publisher_ = this->create_publisher("cmd_vel", 10); 19 | timer_ = this->create_wall_timer( 20 | std::chrono::milliseconds(500), 21 | std::bind(&Acceleration_node::timer_callback, this)); 22 | } 23 | 24 | void timer_callback() 25 | { 26 | auto current_time = this->now(); 27 | auto current_velocity = this->get_parameter("linear_velocity").as_double(); 28 | 29 | // calculate acceleration 30 | double dt = (current_time - last_time_).seconds(); 31 | double acceleration = (current_velocity - last_velocity_) / dt; 32 | 33 | RCLCPP_INFO(this->get_logger(), "Acceleration: %f m/s^2", acceleration); 34 | 35 | // update last velocity and time 36 | last_velocity_ = current_velocity; 37 | last_time_ = current_time; 38 | // Create and publish the Twist message for robot motion 39 | auto message = geometry_msgs::msg::Twist(); 40 | message.linear.x = current_velocity; 41 | publisher_->publish(message); 42 | } 43 | 44 | private: 45 | rclcpp::TimerBase::SharedPtr timer_; 46 | rclcpp::Publisher::SharedPtr publisher_; 47 | double last_velocity_; 48 | rclcpp::Time last_time_; 49 | }; 50 | 51 | int main(int argc, char ** argv) 52 | { 53 | rclcpp::init(argc, argv); 54 | rclcpp::spin(std::make_shared()); 55 | rclcpp::shutdown(); 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /drive_tb3/src/p3_b_angular_velocity.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p3_b_angular_velocity.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node calculates the angular velocity for a robot moving in a circle 5 | * of a specified radius and publishes Twist messages for robot motion. 6 | * @organization Robotisim 7 | */ 8 | 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "geometry_msgs/msg/twist.hpp" 11 | 12 | class Radius_motion : public rclcpp::Node 13 | { 14 | public: 15 | Radius_motion() 16 | : Node("Radius_motion_node") 17 | { 18 | this->declare_parameter("linear_velocity", 1.1); 19 | this->declare_parameter("radius", 1.0); 20 | 21 | publisher_ = this->create_publisher("cmd_vel", 10); 22 | timer_ = this->create_wall_timer( 23 | std::chrono::milliseconds(500), 24 | std::bind(&Radius_motion::timer_callback, this)); 25 | } 26 | 27 | void timer_callback() 28 | { 29 | double linear_velocity =this->get_parameter("linear_velocity").as_double(); 30 | double radius = this->get_parameter("radius").as_double(); 31 | RCLCPP_INFO(this->get_logger(), "VEL_Linear: %f - Radius: %f", linear_velocity,radius); 32 | // Calculate angular velocity and create the Twist message 33 | auto message = geometry_msgs::msg::Twist(); 34 | message.linear.x =linear_velocity; 35 | message.angular.z = linear_velocity/radius; 36 | publisher_->publish(message); 37 | } 38 | 39 | private: 40 | rclcpp::TimerBase::SharedPtr timer_; 41 | rclcpp::Publisher::SharedPtr publisher_; 42 | }; 43 | 44 | int main(int argc, char ** argv) 45 | { 46 | rclcpp::init(argc, argv); 47 | rclcpp::spin(std::make_shared()); 48 | rclcpp::shutdown(); 49 | return 0; 50 | } 51 | -------------------------------------------------------------------------------- /drive_tb3/src/p3_c_linear_goal.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p3_c_linear_goal.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node calculates the error between the current and goal positions in the x-direction 5 | * and publishes Twist messages to control robot motion accordingly. 6 | * @organization Robotisim 7 | */ 8 | 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | class Linear_goal_node : public rclcpp::Node 17 | { 18 | public: 19 | Linear_goal_node() 20 | : Node("linear_goal_node") 21 | { 22 | this->declare_parameter("goal_x", 1.0); 23 | publisher_ = this->create_publisher("/cmd_vel", 10); 24 | subscription_ = this->create_subscription( 25 | "odom", 10, std::bind(&Linear_goal_node::odom_callback, this, std::placeholders::_1)); 26 | } 27 | 28 | private: 29 | void odom_callback(const nav_msgs::msg::Odometry::SharedPtr tb3_odom_msg) 30 | { 31 | auto message = geometry_msgs::msg::Twist(); 32 | double goal_x = this->get_parameter("goal_x").as_double(); 33 | double current_x = tb3_odom_msg->pose.pose.position.x; 34 | double Kp = 2.0; 35 | // Publish error and control robot motion 36 | double error = Kp*(goal_x - current_x) ; 37 | 38 | 39 | // if(error < 0.005){ 40 | // RCLCPP_INFO(this->get_logger(), "Goal Reached"); 41 | 42 | // message.linear.x =0.0; 43 | // publisher_->publish(message); 44 | // }else{ 45 | // RCLCPP_INFO(this->get_logger(), "Error : %f", error); 46 | // message.linear.x =error; 47 | // publisher_->publish(message); 48 | // } 49 | 50 | // Understanding Proportional Controller 51 | RCLCPP_INFO(this->get_logger(), "Error : %f", error); 52 | message.linear.x =error; 53 | publisher_->publish(message); 54 | 55 | } 56 | 57 | rclcpp::Publisher::SharedPtr publisher_; 58 | rclcpp::Subscription::SharedPtr subscription_; 59 | 60 | }; 61 | 62 | int main(int argc, char ** argv) 63 | { 64 | rclcpp::init(argc, argv); 65 | rclcpp::spin(std::make_shared()); 66 | rclcpp::shutdown(); 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /drive_tb3/src/p3_d_xy_goal.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p3_d_xy_goal.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node calculates the error between the current and goal positions in the x and y directions, 5 | * and the error in the yaw angle, and publishes Twist messages to control robot motion accordingly. 6 | * @organization Robotisim 7 | */ 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "tf2/LinearMath/Quaternion.h" 14 | #include "tf2/LinearMath/Matrix3x3.h" 15 | #include 16 | 17 | class Linear_goal_node : public rclcpp::Node 18 | { 19 | public: 20 | Linear_goal_node() 21 | : Node("linear_goal_node") 22 | { 23 | this->declare_parameter("goal_x", 2.0); 24 | this->declare_parameter("goal_y", 3.0); 25 | this->declare_parameter("kp", 5.0); 26 | 27 | 28 | publisher_ = this->create_publisher("/cmd_vel", 10); 29 | subscription_ = this->create_subscription( 30 | "odom", 10, std::bind(&Linear_goal_node::odom_callback, this, std::placeholders::_1)); 31 | } 32 | 33 | private: 34 | void odom_callback(const nav_msgs::msg::Odometry::SharedPtr tb3_odom_msg) 35 | { 36 | // Defineing all variables 37 | auto cmd_msg = geometry_msgs::msg::Twist(); 38 | goal_x = this->get_parameter("goal_x").as_double(); // Set your goal x 39 | goal_y = this->get_parameter("goal_y").as_double(); // Set your goal y 40 | Kp_ = this->get_parameter("kp").as_double(); // Set the proportional gain for control 41 | // Get Robot Pose 42 | float robot_pos_x = tb3_odom_msg->pose.pose.position.x; 43 | float robot_pos_y = tb3_odom_msg->pose.pose.position.y; 44 | 45 | // Convert Quaternions to Euler 46 | double roll,pitch,yaw; 47 | tf2::Quaternion q( 48 | tb3_odom_msg->pose.pose.orientation.x, 49 | tb3_odom_msg->pose.pose.orientation.y, 50 | tb3_odom_msg->pose.pose.orientation.z, 51 | tb3_odom_msg->pose.pose.orientation.w); 52 | tf2::Matrix3x3 m(q); 53 | m.getRPY(roll,pitch,yaw); 54 | 55 | // Calcualte Errors 56 | float error_x = goal_x - robot_pos_x; 57 | float error_y = goal_y - robot_pos_y; 58 | 59 | // Equations for Go TO Goal Behaviour 60 | float distance_to_goal = sqrt( pow(error_x,2) + pow(error_y,2)); 61 | float angle_to_goal = atan2(error_y,error_x); 62 | float error_in_angle = angle_to_goal - yaw ; 63 | 64 | 65 | // Logging 66 | RCLCPP_INFO(this->get_logger(), "DG : %f AG : %f EA : %f", distance_to_goal,angle_to_goal , error_in_angle); 67 | 68 | // Robot Motion Commands 69 | 70 | if(abs(error_in_angle)>0.1){ // Not facing the goal 71 | cmd_msg.linear.x =0.0; 72 | cmd_msg.angular.z =Kp_*(error_in_angle); 73 | RCLCPP_INFO(this->get_logger(), "Case # 1 Rotating towards the Goal"); 74 | 75 | }else{ // If we are looking at the goal 76 | 77 | if(distance_to_goal > 0.1){ //if not reached the goal 78 | cmd_msg.linear.x =Kp_*(distance_to_goal); 79 | cmd_msg.angular.z =0.0; 80 | RCLCPP_INFO(this->get_logger(), "Case # 2 Moving Towards the Goal"); 81 | 82 | }else{ // we have reached the goal 83 | cmd_msg.linear.x =0.0; 84 | cmd_msg.angular.z =0.0; 85 | RCLCPP_INFO(this->get_logger(), "Case # 3 Reached the Goal"); 86 | 87 | } 88 | 89 | } 90 | 91 | 92 | publisher_->publish(cmd_msg); 93 | 94 | } 95 | 96 | rclcpp::Publisher::SharedPtr publisher_; 97 | rclcpp::Subscription::SharedPtr subscription_; 98 | float goal_x; 99 | float goal_y; 100 | float Kp_; 101 | }; 102 | 103 | int main(int argc, char ** argv) 104 | { 105 | rclcpp::init(argc, argv); 106 | rclcpp::spin(std::make_shared()); 107 | rclcpp::shutdown(); 108 | return 0; 109 | } 110 | -------------------------------------------------------------------------------- /drive_tb3/src/p5_a_lidar_data_sub.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p5_a_lidar_data_sub.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node receives LaserScan messages, calculates the distances to the closest obstacles 5 | * in the front, right, and left directions, and publishes Twist messages to control robot motion. 6 | * @organization Robotisim 7 | */ 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | class lidar_subscriber : public rclcpp::Node 16 | { 17 | public: 18 | lidar_subscriber() 19 | : Node("lidar_subscriber") 20 | { 21 | publisher_ = this->create_publisher("/maze_solver/cmd_vel", 10); 22 | subscription_ = this->create_subscription( 23 | "/maze_solver/scan", 10, std::bind(&lidar_subscriber::lidar_callback, this, std::placeholders::_1)); 24 | } 25 | 26 | private: 27 | void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr lidar_msg) 28 | { 29 | // Group obstacles 30 | float right_obstacle = *std::min_element(lidar_msg->ranges.begin() + 260, lidar_msg->ranges.begin() + 280); 31 | float front_obstacle = *std::min_element(lidar_msg->ranges.begin() + 340, lidar_msg->ranges.begin() + 360); 32 | float left_obstacle= *std::min_element(lidar_msg->ranges.begin() + 80, lidar_msg->ranges.begin() + 100); 33 | // Publish distances to the obstacles 34 | RCLCPP_INFO(this->get_logger(), "Front: %f, Right: %f, Left: %f,", front_obstacle, right_obstacle, left_obstacle); 35 | 36 | 37 | } 38 | 39 | rclcpp::Publisher::SharedPtr publisher_; 40 | rclcpp::Subscription::SharedPtr subscription_; 41 | }; 42 | 43 | int main(int argc, char ** argv) 44 | { 45 | rclcpp::init(argc, argv); 46 | rclcpp::spin(std::make_shared()); 47 | rclcpp::shutdown(); 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /drive_tb3/src/p5_b_maze_solving.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p5_b_maze_solving.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node receives LaserScan messages, determines the robot's state based on the 5 | * distances to the closest obstacles, and publishes Twist messages to control robot motion accordingly. 6 | * @organization Robotisim 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | enum class RobotState{ 18 | MOVING_STRAIGHT, 19 | TURNING_LEFT, 20 | TURNING_RIGHT, 21 | OUT_OF_MAZE 22 | }; 23 | 24 | 25 | 26 | class lidar_subscriber : public rclcpp::Node 27 | { 28 | public: 29 | lidar_subscriber() 30 | : Node("lidar_subscriber") 31 | { 32 | publisher_ = this->create_publisher("/maze_solver/cmd_vel", 10); 33 | subscription_ = this->create_subscription( 34 | "/maze_solver/scan", 10, std::bind(&lidar_subscriber::lidar_callback, this, std::placeholders::_1)); 35 | } 36 | 37 | private: 38 | void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr lidar_msg) 39 | { 40 | // Determine robot's state and control robot motion accordingly 41 | float right_obstacle = *std::min_element(lidar_msg->ranges.begin() + 260, lidar_msg->ranges.begin() + 280); 42 | float front_obstacle = *std::min_element(lidar_msg->ranges.begin() + 340, lidar_msg->ranges.begin() + 360); 43 | float left_obstacle= *std::min_element(lidar_msg->ranges.begin() + 80, lidar_msg->ranges.begin() + 100); 44 | RCLCPP_INFO(this->get_logger(), "Front: %f, Right: %f, Left: %f,", front_obstacle, right_obstacle, left_obstacle); 45 | 46 | if(front_obstacle == std::numeric_limits::infinity() && right_obstacle == std::numeric_limits::infinity() && 47 | left_obstacle == std::numeric_limits::infinity() ){ 48 | state_ = RobotState::OUT_OF_MAZE; 49 | } 50 | 51 | switch (state_) 52 | { 53 | case RobotState::MOVING_STRAIGHT: 54 | if(front_obstacle < front_threshold){ 55 | // take a turn 56 | if(left_obstacle < right_obstacle){ 57 | state_=RobotState::TURNING_RIGHT; 58 | }else{ 59 | state_=RobotState::TURNING_LEFT; 60 | } 61 | } 62 | break; 63 | case RobotState::TURNING_LEFT: 64 | if(front_obstacle > front_threshold){ 65 | state_ = RobotState::MOVING_STRAIGHT; 66 | } 67 | break; 68 | case RobotState::TURNING_RIGHT: 69 | if(front_obstacle > front_threshold){ 70 | state_ = RobotState::MOVING_STRAIGHT; 71 | } 72 | break; 73 | 74 | } 75 | 76 | switch (state_) 77 | { 78 | case RobotState::MOVING_STRAIGHT: 79 | command.linear.x = linear_vel; 80 | command.angular.z = 0.0; 81 | RCLCPP_INFO(this->get_logger(), "Moving straight"); 82 | break; 83 | 84 | case RobotState::TURNING_LEFT: 85 | command.linear.x = 0.0; 86 | command.angular.z = angular_vel; 87 | RCLCPP_INFO(this->get_logger(), "Turning left"); 88 | break; 89 | 90 | case RobotState::TURNING_RIGHT: 91 | command.linear.x = 0.0; 92 | command.angular.z = -angular_vel; 93 | RCLCPP_INFO(this->get_logger(), "Turning right"); 94 | break; 95 | 96 | case RobotState::OUT_OF_MAZE: 97 | command.linear.x = 0.0; 98 | command.angular.z = 0.0; 99 | RCLCPP_INFO(this->get_logger(), "Out of maze, stopping"); 100 | break; 101 | } 102 | 103 | publisher_->publish(command); 104 | 105 | } 106 | double front_threshold = 1.5; 107 | double angular_vel = 0.8; 108 | double linear_vel = 0.6; 109 | geometry_msgs::msg::Twist command; 110 | RobotState state_; 111 | rclcpp::Publisher::SharedPtr publisher_; 112 | rclcpp::Subscription::SharedPtr subscription_; 113 | }; 114 | 115 | int main(int argc, char ** argv) 116 | { 117 | rclcpp::init(argc, argv); 118 | rclcpp::spin(std::make_shared()); 119 | rclcpp::shutdown(); 120 | return 0; 121 | } 122 | -------------------------------------------------------------------------------- /drive_tb3/src/p5_c_camera_data_sub.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p5_c_camera_data_sub.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node receives Image messages, converts the images to grayscale, and displays the images. 5 | * @organization Robotisim 6 | */ 7 | #include "rclcpp/rclcpp.hpp" 8 | #include "sensor_msgs/msg/image.hpp" 9 | #include "geometry_msgs/msg/twist.hpp" 10 | #include "cv_bridge/cv_bridge.h" 11 | #include "opencv2/opencv.hpp" 12 | 13 | class camera_subscriber : public rclcpp::Node 14 | { 15 | public: 16 | camera_subscriber() 17 | : Node("camera_subscriber_node") 18 | { 19 | this->declare_parameter("lower_threshold",200); 20 | this->declare_parameter("upper_threshold",250); 21 | 22 | subscription_ = this->create_subscription( 23 | "/camera/image_raw", 10, std::bind(&camera_subscriber::camera_callback, this, std::placeholders::_1)); 24 | RCLCPP_INFO(this->get_logger(), "------ Node Started -----"); 25 | } 26 | 27 | 28 | 29 | private: 30 | int count_1_edge=0;int count_2_edge=0; int count_3_edge=0; int count_4_edge=0; 31 | 32 | void camera_callback(const sensor_msgs::msg::Image::SharedPtr camera_msg) 33 | { 34 | cv_bridge::CvImagePtr cv_ptr; 35 | cv_ptr = cv_bridge::toCvCopy(camera_msg,"bgr8"); 36 | cv::Mat gray_image, canny_image; 37 | cv::cvtColor(cv_ptr->image, gray_image, cv::COLOR_BGR2GRAY); 38 | 39 | cv::imshow("Image",gray_image); 40 | cv::waitKey(1); 41 | } 42 | 43 | rclcpp::Publisher::SharedPtr publisher_; 44 | rclcpp::Subscription::SharedPtr subscription_; 45 | rclcpp::TimerBase::SharedPtr timer_; 46 | }; 47 | 48 | 49 | int main(int argc, char ** argv) 50 | { 51 | 52 | 53 | rclcpp::init(argc, argv); 54 | rclcpp::spin(std::make_shared()); 55 | rclcpp::shutdown(); 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /drive_tb3/src/p5_d_line_following.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p5_d_line_following.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node receives Image messages, applies the Canny edge detection algorithm to the images, 5 | * finds the midpoint of the detected line, calculates the error between the midpoint and the center of the image, 6 | * and publishes Twist messages to control robot motion. 7 | * @organization Robotisim 8 | */ 9 | #include "rclcpp/rclcpp.hpp" 10 | #include "sensor_msgs/msg/image.hpp" 11 | #include "geometry_msgs/msg/twist.hpp" 12 | #include "cv_bridge/cv_bridge.h" 13 | #include "opencv2/opencv.hpp" 14 | 15 | class camera_subscriber : public rclcpp::Node 16 | { 17 | public: 18 | camera_subscriber() 19 | : Node("camera_subscriber_node") 20 | { 21 | this->declare_parameter("lower_threshold",200); 22 | this->declare_parameter("upper_threshold",250); 23 | publisher_ = this->create_publisher("cmd_vel", 10); 24 | subscription_ = this->create_subscription( 25 | "/camera/image_raw", 10, std::bind(&camera_subscriber::camera_callback, this, std::placeholders::_1)); 26 | RCLCPP_INFO(this->get_logger(), "\n------ Node Started -----\n"); 27 | } 28 | 29 | 30 | 31 | private: 32 | int count_1_edge=0;int count_2_edge=0; int count_3_edge=0; int count_4_edge=0; 33 | 34 | void camera_callback(const sensor_msgs::msg::Image::SharedPtr camera_msg) 35 | { 36 | auto velocity_msg = geometry_msgs::msg::Twist(); 37 | cv_bridge::CvImagePtr cv_ptr; 38 | cv_ptr = cv_bridge::toCvCopy(camera_msg,"bgr8"); 39 | cv::Mat gray_image, canny_image; 40 | cv::cvtColor(cv_ptr->image, gray_image, cv::COLOR_BGR2GRAY); 41 | 42 | int upper_threshold= this->get_parameter("upper_threshold").as_int(); 43 | int lower_threshold= this->get_parameter("lower_threshold").as_int(); 44 | cv::Canny(gray_image,canny_image,lower_threshold,upper_threshold); 45 | 46 | int row=150,column=0; 47 | // Apply Canny edge detection and find midpoint of the line 48 | cv::Mat roi = canny_image(cv::Range(row,row+240) , cv::Range(column , column+640)); 49 | 50 | 51 | std::vector edge; 52 | for(int i=0;i<640;++i){ 53 | if(roi.at(160,i)==255){ 54 | edge.push_back(i); 55 | } 56 | } 57 | 58 | // Finding Mid point 59 | int mid_area = edge[1] - edge[0]; 60 | int mid_point = edge[0] + mid_area/2; 61 | int robot_mid_point = 640/2; 62 | 63 | cv::circle(roi,cv::Point(mid_point,160),2,cv::Scalar(255,255,255),-1); 64 | cv::circle(roi,cv::Point(robot_mid_point,160),5,cv::Scalar(255,255,255),-1); 65 | 66 | // Control Algorithms 67 | velocity_msg.linear.x=0.1; 68 | double error = robot_mid_point - mid_point; 69 | if(error<0){ 70 | RCLCPP_INFO(this->get_logger(), "Turn Right"); 71 | velocity_msg.angular.z=-angular_vel; 72 | } 73 | else { 74 | RCLCPP_INFO(this->get_logger(), "Turn Left"); 75 | velocity_msg.angular.z=angular_vel; 76 | } 77 | 78 | publisher_->publish(velocity_msg); 79 | 80 | cv::imshow("Image",roi); 81 | cv::waitKey(1); 82 | } 83 | 84 | rclcpp::Publisher::SharedPtr publisher_; 85 | rclcpp::Subscription::SharedPtr subscription_; 86 | rclcpp::TimerBase::SharedPtr timer_; 87 | double angular_vel=0.3; 88 | }; 89 | 90 | 91 | int main(int argc, char ** argv) 92 | { 93 | 94 | 95 | rclcpp::init(argc, argv); 96 | rclcpp::spin(std::make_shared()); 97 | rclcpp::shutdown(); 98 | return 0; 99 | } 100 | -------------------------------------------------------------------------------- /drive_tb3/src/p6_a_occupancy_grid.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p6_a_occupancy_grid.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node publishes OccupancyGrid messages at a regular interval. 5 | * @organization Robotisim 6 | */ 7 | #include 8 | #include 9 | #include 10 | #include "nav_msgs/msg/occupancy_grid.hpp" 11 | #include "std_msgs/msg/header.hpp" 12 | #include 13 | using namespace std::chrono_literals; 14 | 15 | class OccupancyGridPub : public rclcpp::Node 16 | { 17 | public: 18 | OccupancyGridPub() 19 | : Node("OccupancyGridPub") 20 | { 21 | this->declare_parameter("goal_x", 1.0); 22 | publisher_ = this->create_publisher("/map", 10); 23 | timer_ = this->create_wall_timer( 24 | 500ms, std::bind(&OccupancyGridPub::OG_callback, this)); 25 | } 26 | 27 | private: 28 | void OG_callback() 29 | { 30 | // Populate OccupancyGrid message 31 | auto occupancy_grid_message = nav_msgs::msg::OccupancyGrid(); 32 | occupancy_grid_message.header =std_msgs::msg::Header(); 33 | occupancy_grid_message.header.stamp = this->now(); 34 | occupancy_grid_message.header.frame_id ="Map_frame"; 35 | 36 | occupancy_grid_message.info.resolution =1.0; 37 | occupancy_grid_message.info.width = 3; 38 | occupancy_grid_message.info.height =3; 39 | 40 | occupancy_grid_message.info.origin.position.x = 0.0; 41 | occupancy_grid_message.info.origin.position.y = 0.0; 42 | occupancy_grid_message.info.origin.position.z = 0.0; 43 | 44 | std::vector data = {0,0,0,0,1,0,0,0,-1}; 45 | occupancy_grid_message.data =data; 46 | 47 | publisher_->publish(occupancy_grid_message); 48 | 49 | } 50 | 51 | rclcpp::Publisher::SharedPtr publisher_; 52 | rclcpp::TimerBase::SharedPtr timer_; 53 | 54 | }; 55 | 56 | int main(int argc, char ** argv) 57 | { 58 | rclcpp::init(argc, argv); 59 | rclcpp::spin(std::make_shared()); 60 | rclcpp::shutdown(); 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /drive_tb3/src/p6_b_sdf_spawner.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p6_b_sdf_spawner.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node reads a file into a string and sends a service request to spawn an entity in a simulation. 5 | * @organization Robotisim 6 | */ 7 | #include "rclcpp/rclcpp.hpp" 8 | #include "gazebo_msgs/srv/spawn_entity.hpp" 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // Function to read a file into a string 17 | std::string readFile(const std::string& filePath) { 18 | std::ifstream fileStream(filePath); 19 | return std::string((std::istreambuf_iterator(fileStream)), 20 | std::istreambuf_iterator()); 21 | } 22 | 23 | int main(int argc, char * argv[]) { 24 | // Initialize ROS2 25 | rclcpp::init(argc, argv); 26 | 27 | // Check for correct number of arguments 28 | if(argc < 3){ 29 | std::cout << "Provide path to sdf and a name for the entity" << std::endl; 30 | return -1; 31 | } 32 | 33 | // Create node and client 34 | auto node = rclcpp::Node::make_shared("Spawning_Node"); 35 | auto client = node->create_client("/spawn_entity"); 36 | 37 | // Wait for service 38 | while (!client->wait_for_service(std::chrono::seconds(1))) { 39 | if (!rclcpp::ok()) { 40 | RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); 41 | return 0; 42 | } 43 | RCLCPP_INFO(node->get_logger(), "Waiting for service to appear..."); 44 | } 45 | RCLCPP_INFO(node->get_logger(), "Connected to spawner"); 46 | 47 | // Create and populate request 48 | auto request = std::make_shared(); 49 | request->name = argv[2]; 50 | request->xml = readFile(argv[1]); 51 | if(argc > 4){ 52 | request->initial_pose.position.x = std::stof(argv[3]); 53 | request->initial_pose.position.y = std::stof(argv[4]); 54 | } 55 | 56 | // Send request 57 | RCLCPP_INFO(node->get_logger(), "Sending service request to `/spawn_entity`"); 58 | auto result_future = client->async_send_request(request); 59 | 60 | // Handle response 61 | if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::TIMEOUT) 62 | { 63 | RCLCPP_INFO(node->get_logger(), "Response: %s", result_future.get()->status_message.c_str()); 64 | } else { 65 | RCLCPP_ERROR(node->get_logger(), "Failed to call service /spawn_entity"); 66 | } 67 | 68 | // Shutdown 69 | RCLCPP_INFO(node->get_logger(), "Done! Shutting down node."); 70 | rclcpp::shutdown(); 71 | 72 | return 0; 73 | } 74 | -------------------------------------------------------------------------------- /drive_tb3/src/p6_c_commander_api_go_to_pose.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | # Copyright 2021 Samsung Research America 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | 17 | from copy import deepcopy 18 | 19 | from geometry_msgs.msg import PoseStamped 20 | from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult 21 | import rclpy 22 | 23 | """ 24 | Basic stock inspection demo. In this demonstration, the expectation 25 | is that there are cameras or RFID sensors mounted on the robots 26 | collecting information about stock quantity and location. 27 | """ 28 | 29 | 30 | def main(): 31 | rclpy.init() 32 | 33 | navigator = BasicNavigator() 34 | 35 | # Inspection route, probably read in from a file for a real application 36 | # from either a map or drive and repeat. 37 | inspection_route = [ 38 | [3.461, -0.450], 39 | [5.531, -0.450], 40 | [3.461, -2.200], 41 | [5.531, -2.200], 42 | [3.661, -4.121], 43 | [5.431, -4.121], 44 | [3.661, -5.850], 45 | [5.431, -5.800]] 46 | 47 | # Set our demo's initial pose 48 | initial_pose = PoseStamped() 49 | initial_pose.header.frame_id = 'map' 50 | initial_pose.header.stamp = navigator.get_clock().now().to_msg() 51 | initial_pose.pose.position.x = -6.100 52 | initial_pose.pose.position.y = -7.10 53 | initial_pose.pose.orientation.z = 0.706 54 | initial_pose.pose.orientation.w = 0.707 55 | navigator.setInitialPose(initial_pose) 56 | 57 | # Wait for navigation to fully activate 58 | navigator.waitUntilNav2Active() 59 | 60 | # Send our route 61 | inspection_points = [] 62 | inspection_pose = PoseStamped() 63 | inspection_pose.header.frame_id = 'map' 64 | inspection_pose.header.stamp = navigator.get_clock().now().to_msg() 65 | inspection_pose.pose.orientation.z = 1.0 66 | inspection_pose.pose.orientation.w = 0.0 67 | for pt in inspection_route: 68 | inspection_pose.pose.position.x = pt[0] 69 | inspection_pose.pose.position.y = pt[1] 70 | inspection_points.append(deepcopy(inspection_pose)) 71 | navigator.followWaypoints(inspection_points) 72 | 73 | # Do something during our route (e.x. AI to analyze stock information or upload to the cloud) 74 | # Simply the current waypoint ID for the demonstation 75 | i = 0 76 | while not navigator.isTaskComplete(): 77 | i += 1 78 | feedback = navigator.getFeedback() 79 | if feedback and i % 5 == 0: 80 | print('Executing current waypoint: ' + 81 | str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points))) 82 | 83 | result = navigator.getResult() 84 | if result == TaskResult.SUCCEEDED: 85 | print('Inspection of shelves complete! Returning to start...') 86 | elif result == TaskResult.CANCELED: 87 | print('Inspection of shelving was canceled. Returning to start...') 88 | elif result == TaskResult.FAILED: 89 | print('Inspection of shelving failed! Returning to start...') 90 | 91 | # go back to start 92 | initial_pose.header.stamp = navigator.get_clock().now().to_msg() 93 | navigator.goToPose(initial_pose) 94 | while not navigator.isTaskComplete(): 95 | pass 96 | 97 | exit(0) 98 | 99 | 100 | if __name__ == '__main__': 101 | main() 102 | -------------------------------------------------------------------------------- /drive_tb3/worlds/line_following.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 0 17 | 0 18 | 0 19 | 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | 0 0 1 28 | 100 100 29 | 30 | 31 | 32 | 33 | 34 | 100 35 | 50 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 10 47 | 48 | 49 | 0 50 | 51 | 52 | 0 0 1 53 | 100 100 54 | 55 | 56 | 57 | 61 | 62 | 63 | 0 64 | 0 65 | 0 66 | 67 | 68 | 0 0 -9.8 69 | 6e-06 2.3e-05 -4.2e-05 70 | 71 | 72 | 0.001 73 | 1 74 | 1000 75 | 76 | 77 | 0.4 0.4 0.4 1 78 | 0.7 0.7 0.7 1 79 | 1 80 | 81 | 84 | 85 | 86 | EARTH_WGS84 87 | 0 88 | 0 89 | 0 90 | 0 91 | 92 | 93 | 94 | 95 | 1 96 | 97 | 0.166667 98 | 0 99 | 0 100 | 0.166667 101 | 0 102 | 0.166667 103 | 104 | 105 | 0.848639 -0.049037 0 0 -0 0 106 | 107 | 0 0 0 0 -0 0 108 | 109 | 110 | /home/luqman/robotisim_ws/src/mobile_robotics_ROS2/drive_tb3/models/meshes/base.dae 111 | 1 1 1 112 | 113 | 114 | 115 | 1 116 | 120 | 121 | 0 0 0 1 122 | 123 | 0 124 | 1 125 | 126 | 127 | 0 128 | 10 129 | 0 0 0 0 -0 0 130 | 131 | 132 | /home/luqman/robotisim_ws/src/mobile_robotics_ROS2/drive_tb3/models/meshes/base.dae 133 | 1 1 1 134 | 135 | 136 | 137 | 138 | 139 | 1 140 | 1 141 | 0 0 0 142 | 0 143 | 0 144 | 145 | 146 | 1 147 | 0 148 | 0 149 | 1 150 | 151 | 0 152 | 153 | 154 | 155 | 156 | 0 157 | 1e+06 158 | 159 | 160 | 0 161 | 1 162 | 1 163 | 164 | 0 165 | 0.2 166 | 1e+13 167 | 1 168 | 0.01 169 | 0 170 | 171 | 172 | 1 173 | -0.01 174 | 0 175 | 0.2 176 | 1e+13 177 | 1 178 | 179 | 180 | 181 | 182 | 0 183 | 0 184 | 0 185 | 186 | 187 | 188 | 1 189 | 190 | 0.166667 191 | 0 192 | 0 193 | 0.166667 194 | 0 195 | 0.166667 196 | 197 | 198 | -0.848635 0.049036 0.001 0 -0 0 199 | 200 | 0 0 0 0 -0 0 201 | 202 | 203 | /home/luqman/robotisim_ws/src/mobile_robotics_ROS2/drive_tb3/models/meshes/line.dae 204 | 1 1 1 205 | 206 | 207 | 208 | 1 209 | 213 | 214 | 0 0 0 1 215 | 216 | 0 217 | 1 218 | 219 | 220 | 0 221 | 10 222 | 0 0 0 0 -0 0 223 | 224 | 225 | /home/luqman/robotisim_ws/src/mobile_robotics_ROS2/drive_tb3/models/meshes/line.dae 226 | 1 1 1 227 | 228 | 229 | 230 | 231 | 232 | 1 233 | 1 234 | 0 0 0 235 | 0 236 | 0 237 | 238 | 239 | 1 240 | 0 241 | 0 242 | 1 243 | 244 | 0 245 | 246 | 247 | 248 | 249 | 0 250 | 1e+06 251 | 252 | 253 | 0 254 | 1 255 | 1 256 | 257 | 0 258 | 0.2 259 | 1e+13 260 | 1 261 | 0.01 262 | 0 263 | 264 | 265 | 1 266 | -0.01 267 | 0 268 | 0.2 269 | 1e+13 270 | 1 271 | 272 | 273 | 274 | 275 | 0 276 | 0 277 | 0 278 | 279 | 280 | link_0 281 | link_1 282 | 0 0 0 0 -0 0 283 | 284 | 285 | 286 | 0 287 | 0.2 288 | 289 | 290 | 0 291 | 0.2 292 | 293 | 294 | 295 | 296 | 1 297 | 1 298 | -2.86298 2.05919 0 0 -0 0 299 | 300 | 301 | 36 805000000 302 | 36 969864950 303 | 1684771051 33299960 304 | 36805 305 | 306 | 0 0 0 0 -0 0 307 | 1 1 1 308 | 309 | 0 0 0 0 -0 0 310 | 0 0 0 0 -0 0 311 | 0 0 0 0 -0 0 312 | 0 0 0 0 -0 0 313 | 314 | 315 | 316 | -2.86298 2.05919 0 0 -0 0 317 | 1 1 1 318 | 319 | -2.01434 2.01015 0 0 -0 0 320 | 0 0 0 0 -0 0 321 | 0 0 0 0 -0 0 322 | 0 0 0 0 -0 0 323 | 324 | 325 | -3.71162 2.10823 -0.029 0 -0 0 326 | 0 0 0 0 -0 0 327 | 0 0 0 0 -0 0 328 | 0 0 0 0 -0 0 329 | 330 | 331 | 332 | 0 0 10 0 -0 0 333 | 334 | 335 | 336 | 337 | 5.06079 -0.635196 1.50902 0 0.279643 3.11219 338 | orbit 339 | perspective 340 | 341 | 342 | 343 | 344 | -------------------------------------------------------------------------------- /drive_tb3/worlds/maze.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 0 17 | 0 18 | 0 19 | 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | 0 0 1 28 | 100 100 29 | 30 | 31 | 32 | 33 | 34 | 100 35 | 50 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 10 47 | 48 | 49 | 0 50 | 51 | 52 | 0 0 1 53 | 100 100 54 | 55 | 56 | 57 | 61 | 62 | 63 | 0 64 | 0 65 | 0 66 | 67 | 68 | 0 0 -9.8 69 | 6e-06 2.3e-05 -4.2e-05 70 | 71 | 72 | 0.001 73 | 1 74 | 1000 75 | 76 | 77 | 0.4 0.4 0.4 1 78 | 0.7 0.7 0.7 1 79 | 0 80 | 81 | 84 | 85 | 86 | EARTH_WGS84 87 | 0 88 | 0 89 | 0 90 | 0 91 | 92 | 93 | -0.613508 -1.19005 0 0 -0 0 94 | 95 | 96 | 97 | 98 | 15 0.15 2.5 99 | 100 | 101 | 0 0 1.25 0 -0 0 102 | 10 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 0 0 1.25 0 -0 0 118 | 119 | 120 | 15 0.15 2.5 121 | 122 | 123 | 124 | 128 | 1 1 1 1 129 | 130 | 131 | 0 132 | 133 | 134 | 1.04235 -0.948542 0 0 -0 0 135 | 0 136 | 0 137 | 0 138 | 139 | 140 | 141 | 142 | 143 | 9.5 0.15 2.5 144 | 145 | 146 | 0 0 1.25 0 -0 0 147 | 10 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 0 0 1.25 0 -0 0 163 | 164 | 165 | 9.5 0.15 2.5 166 | 167 | 168 | 169 | 173 | 1 1 1 1 174 | 175 | 176 | 0 177 | 178 | 179 | 8.46735 3.72646 0 0 -0 1.5708 180 | 0 181 | 0 182 | 0 183 | 184 | 185 | 186 | 187 | 188 | 9.75 0.15 2.5 189 | 190 | 191 | 0 0 1.25 0 -0 0 192 | 10 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 0 0 1.25 0 -0 0 208 | 209 | 210 | 9.75 0.15 2.5 211 | 212 | 213 | 214 | 218 | 1 1 1 1 219 | 220 | 221 | 0 222 | 223 | 224 | -8.46692 -3.6617 0 0 -0 1.5708 225 | 0 226 | 0 227 | 0 228 | 229 | 230 | 231 | 232 | 233 | 0.15 0.15 2.5 234 | 235 | 236 | 0 0 1.25 0 -0 0 237 | 10 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 0 0 1.25 0 -0 0 253 | 254 | 255 | 0.15 0.15 2.5 256 | 257 | 258 | 259 | 263 | 1 1 1 1 264 | 265 | 266 | 0 267 | 268 | 269 | -8.46692 1.1117 0 0 -0 0 270 | 0 271 | 0 272 | 0 273 | 274 | 275 | 276 | 277 | 278 | 14.6445 0.15 2.5 279 | 280 | 281 | 0 0 1.25 0 -0 0 282 | 10 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 0 0 1.25 0 -0 0 298 | 299 | 300 | 14.6445 0.15 2.5 301 | 302 | 303 | 304 | 308 | 1 1 1 1 309 | 310 | 311 | 0 312 | 313 | 314 | -1.22012 1.1117 0 0 -0 0 315 | 0 316 | 0 317 | 0 318 | 319 | 320 | 321 | 322 | 323 | 7.5 0.15 2.5 324 | 325 | 326 | 0 0 1.25 0 -0 0 327 | 10 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 0 0 1.25 0 -0 0 343 | 344 | 345 | 7.5 0.15 2.5 346 | 347 | 348 | 349 | 353 | 1 1 1 1 354 | 355 | 356 | 0 357 | 358 | 359 | 6.02668 4.7867 0 0 -0 1.5708 360 | 0 361 | 0 362 | 0 363 | 364 | 365 | 366 | 367 | 368 | 7.5 0.15 2.5 369 | 370 | 371 | 0 0 1.25 0 -0 0 372 | 10 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | 384 | 385 | 386 | 387 | 0 0 1.25 0 -0 0 388 | 389 | 390 | 7.5 0.15 2.5 391 | 392 | 393 | 394 | 398 | 1 1 1 1 399 | 400 | 401 | 0 402 | 403 | 404 | -6.38265 -4.62354 0 0 -0 1.5708 405 | 0 406 | 0 407 | 0 408 | 409 | 1 410 | 411 | 412 | 61 173000000 413 | 61 424925989 414 | 1683701667 774622929 415 | 61173 416 | 417 | 0 0 0 0 -0 0 418 | 1 1 1 419 | 420 | 0 0 0 0 -0 0 421 | 0 0 0 0 -0 0 422 | 0 0 0 0 -0 0 423 | 0 0 0 0 -0 0 424 | 425 | 426 | 427 | -0.613508 -0.377942 0 0 -0 0 428 | 1 1 1 429 | 430 | 0.428842 -1.32648 0 0 -0 0 431 | 0 0 0 0 -0 0 432 | 0 0 0 0 -0 0 433 | 0 0 0 0 -0 0 434 | 435 | 436 | 7.85384 3.34852 0 0 -0 1.5708 437 | 0 0 0 0 -0 0 438 | 0 0 0 0 -0 0 439 | 0 0 0 0 -0 0 440 | 441 | 442 | -9.08043 -4.03964 0 0 -0 1.5708 443 | 0 0 0 0 -0 0 444 | 0 0 0 0 -0 0 445 | 0 0 0 0 -0 0 446 | 447 | 448 | -9.08043 0.733758 0 0 -0 0 449 | 0 0 0 0 -0 0 450 | 0 0 0 0 -0 0 451 | 0 0 0 0 -0 0 452 | 453 | 454 | -1.83363 0.733758 0 0 -0 0 455 | 0 0 0 0 -0 0 456 | 0 0 0 0 -0 0 457 | 0 0 0 0 -0 0 458 | 459 | 460 | 5.41317 4.40876 0 0 -0 1.5708 461 | 0 0 0 0 -0 0 462 | 0 0 0 0 -0 0 463 | 0 0 0 0 -0 0 464 | 465 | 466 | -6.99616 -5.00148 0 0 -0 1.5708 467 | 0 0 0 0 -0 0 468 | 0 0 0 0 -0 0 469 | 0 0 0 0 -0 0 470 | 471 | 472 | 473 | 0 0 10 0 -0 0 474 | 475 | 476 | 477 | 478 | -0.985581 -6.04947 49.5 0 1.52764 1.5962 479 | orbit 480 | perspective 481 | 482 | 483 | 484 | 485 | -------------------------------------------------------------------------------- /drive_turtle/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(drive_turtle) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(rclcpp REQUIRED) 6 | find_package(turtlesim REQUIRED) 7 | find_package(geometry_msgs REQUIRED) 8 | 9 | add_executable(p1_single_straight_drive src/p1_a_single_straight_drive.cpp) 10 | ament_target_dependencies(p1_single_straight_drive rclcpp turtlesim geometry_msgs) 11 | 12 | 13 | add_executable(p1_multi_straight_drive src/p1_b_multi_straight_drive.cpp) 14 | ament_target_dependencies(p1_multi_straight_drive rclcpp turtlesim geometry_msgs) 15 | 16 | add_executable(p1_multi_turtle_controller src/p1_c_multi_turtle_controller.cpp) 17 | ament_target_dependencies(p1_multi_turtle_controller rclcpp turtlesim geometry_msgs) 18 | 19 | 20 | 21 | install(TARGETS 22 | p1_multi_turtle_controller p1_single_straight_drive p1_multi_straight_drive 23 | DESTINATION lib/${PROJECT_NAME} 24 | ) 25 | install(DIRECTORY launch 26 | DESTINATION share/${PROJECT_NAME} 27 | ) 28 | 29 | 30 | 31 | ament_package() 32 | -------------------------------------------------------------------------------- /drive_turtle/launch/p1_a_multi_robot_multi_sim.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file is used to start two instances of the turtlesim_node, a simple simulator for ROS2. 6 | These two nodes, turtlesim1 and turtlesim2, are placed in different namespaces, robot1 and robot2 respectively, 7 | which allows them to operate independently within the ROS2 system. 8 | 9 | Packages used: 10 | - turtlesim 11 | """ 12 | 13 | from launch import LaunchDescription 14 | from launch_ros.actions import Node 15 | 16 | def generate_launch_description(): 17 | 18 | ######### Running Turtlesim Node 19 | # Nodes for two turtlesim simulations 20 | turtlesim1 = Node( 21 | package='turtlesim', 22 | executable='turtlesim_node', 23 | name='turtlesim1', 24 | namespace='robot1' 25 | ) 26 | 27 | turtlesim2 = Node( 28 | package='turtlesim', 29 | executable='turtlesim_node', 30 | name='turtlesim2', 31 | namespace='robot2' 32 | ) 33 | 34 | return LaunchDescription([ 35 | turtlesim1, 36 | turtlesim2, 37 | ]) 38 | -------------------------------------------------------------------------------- /drive_turtle/launch/p1_b_multi_robot_single_sim.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file starts a turtlesim_node, kills the first turtle, and then spawns two new turtles at different locations. 6 | 7 | Packages used: 8 | - turtlesim 9 | """ 10 | 11 | from launch import LaunchDescription 12 | from launch_ros.actions import Node 13 | from launch.actions import ExecuteProcess 14 | from launch.actions import TimerAction 15 | 16 | def generate_launch_description(): 17 | # Create a new LaunchDescription 18 | ld = LaunchDescription() 19 | 20 | ##### Starting the turtlesim_node 21 | # This node starts the turtlesim simulation. 22 | bring_sim = Node( 23 | package='turtlesim', 24 | executable='turtlesim_node', 25 | name='turtlesim' 26 | ) 27 | 28 | ##### Killing the first turtle 29 | # This command calls the /kill service to remove the first turtle from the simulation. 30 | kill_first = ExecuteProcess( 31 | cmd=['ros2', 'service', 'call', '/kill', 'turtlesim/srv/Kill', "\"{name: 'turtle1'}\"",], 32 | name='kill_turtle1', 33 | shell=True 34 | ) 35 | 36 | ##### Spawning the first turtle 37 | # This command calls the /spawn service to create a new turtle at coordinates (3, 1) after a delay of 2 seconds. 38 | spawn_turtle_1 = ExecuteProcess( 39 | cmd=['ros2', 'service', 'call', '/spawn', 'turtlesim/srv/Spawn', "\"{x: 3.0, y: 1.0, theta: 1.57, name: 'turtle1'}\"",], 40 | name='spawn_turtle1', 41 | shell=True 42 | ) 43 | 44 | spawn_turtle1_delayed = TimerAction( 45 | period=2.0, 46 | actions=[spawn_turtle_1], 47 | ) 48 | 49 | ##### Spawning the second turtle 50 | # This command calls the /spawn service to create a second turtle at coordinates (2, 1). 51 | spawn_turtle_2 = ExecuteProcess( 52 | cmd=['ros2', 'service', 'call', '/spawn', 'turtlesim/srv/Spawn', "\"{x: 2.0, y: 1.0, theta: 1.57, name: 'turtle2'}\"",], 53 | name='spawn_turtle2', 54 | shell=True 55 | ) 56 | 57 | # Add the nodes and actions to the LaunchDescription 58 | ld.add_action(bring_sim) 59 | ld.add_action(kill_first) 60 | ld.add_action(spawn_turtle1_delayed) 61 | ld.add_action(spawn_turtle_2) 62 | 63 | # Return the LaunchDescription 64 | return ld 65 | -------------------------------------------------------------------------------- /drive_turtle/launch/p1_c_multi_robot_controller.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This launch file starts two controllers for two turtles. Each controller drives its turtle at a different speed. 6 | Packages used: 7 | - turtlesim 8 | """ 9 | 10 | from launch import LaunchDescription 11 | from launch_ros.actions import Node 12 | 13 | def generate_launch_description(): 14 | ld = LaunchDescription() 15 | 16 | ##### Controller for turtle1 17 | # This node runs the p1_multi_turtle_controller executable from the drive_turtle package. 18 | # It controls the turtle named 'turtle1', driving it at a speed of 2.0. 19 | turtle1_controller = Node( 20 | package='drive_turtle', 21 | executable='p1_multi_turtle_controller', 22 | name='turtle1_controller', 23 | parameters=[ 24 | {'cmd_vel_topic': '/turtle1/cmd_vel'}, 25 | {'linear_velocity': 2.5} 26 | ] 27 | ) 28 | 29 | ##### Controller for turtle2 30 | # This node runs the p1_multi_turtle_controller executable from the drive_turtle package. 31 | # It controls the turtle named 'turtle2', driving it at a speed of 1.5. 32 | turtle2_controller = Node( 33 | package='drive_turtle', 34 | executable='p1_multi_turtle_controller', 35 | name='turtle2_controller', 36 | parameters=[ 37 | {'cmd_vel_topic': '/turtle2/cmd_vel'}, 38 | {'linear_velocity': 1.5} 39 | ] 40 | ) 41 | 42 | # Add the controllers to the LaunchDescription 43 | ld.add_action(turtle1_controller) 44 | ld.add_action(turtle2_controller) 45 | 46 | # Return the LaunchDescription 47 | return ld 48 | -------------------------------------------------------------------------------- /drive_turtle/launch/p1_d_run_multi_turtlesim_drive.launch.py: -------------------------------------------------------------------------------- 1 | """ 2 | Author: Muhammad Luqman 3 | Organization: Robotisim 4 | 5 | This file launches two included launch files from the 'drive_turtle' package to initiate a simulation and start robot controllers. 6 | 7 | Packages used: 8 | - turtlesim 9 | """ 10 | 11 | #!/usr/bin/env python3 12 | import os 13 | 14 | from ament_index_python.packages import get_package_share_directory 15 | from launch import LaunchDescription 16 | from launch.actions import IncludeLaunchDescription 17 | from launch.launch_description_sources import PythonLaunchDescriptionSource 18 | from launch.substitutions import LaunchConfiguration 19 | from launch_ros.actions import Node 20 | 21 | def generate_launch_description(): 22 | ##### File Paths are stored 23 | # This line gets the path to the drive_turtle package. 24 | pkg_path = get_package_share_directory('drive_turtle') 25 | 26 | ##### Include the launch file to start the simulation 27 | # This line includes the launch file p1_b_multi_robot_single_sim.launch.py from the drive_turtle package. 28 | bring_sim = IncludeLaunchDescription( 29 | PythonLaunchDescriptionSource( 30 | os.path.join(pkg_path, 'launch', 'p1_b_multi_robot_single_sim.launch.py') 31 | ), 32 | ) 33 | 34 | ##### Include the launch file to start the robot controllers 35 | # This line includes the launch file p1_c_multi_robot_controller.launch.py from the drive_turtle package. 36 | bring_robots = IncludeLaunchDescription( 37 | PythonLaunchDescriptionSource( 38 | os.path.join(pkg_path, 'launch', 'p1_c_multi_robot_controller.launch.py') 39 | ), 40 | ) 41 | 42 | ld = LaunchDescription() 43 | 44 | # Add the IncludeLaunchDescription actions to the LaunchDescription 45 | ld.add_action(bring_sim) 46 | ld.add_action(bring_robots) 47 | 48 | # Return the LaunchDescription 49 | return ld 50 | -------------------------------------------------------------------------------- /drive_turtle/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | drive_turtle 5 | 0.0.0 6 | TODO: Package description 7 | luqman 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /drive_turtle/src/p1_a_single_straight_drive.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file p1_a_single_straight_drive.cpp 3 | * @author Muhammad Luqman 4 | * @brief This ROS2 node continuously publishes Twist messages to a specified topic to make a turtle robot move in a straight line. 5 | * @organization Robotisim 6 | */ 7 | 8 | #include "rclcpp/rclcpp.hpp" 9 | #include "geometry_msgs/msg/twist.hpp" 10 | 11 | // The TurtlesimStraightLine node 12 | class TurtlesimStraightLine : public rclcpp::Node { 13 | public: 14 | TurtlesimStraightLine() : Node("turtlesim_straight_line") { 15 | // Set up a publisher and a timer 16 | cmd_vel_pub_ = this->create_publisher("/robot1/turtle1/cmd_vel", 10); 17 | timer_ = this->create_wall_timer(std::chrono::milliseconds(100), 18 | std::bind(&TurtlesimStraightLine::timer_callback, this)); 19 | } 20 | 21 | private: 22 | rclcpp::Publisher::SharedPtr cmd_vel_pub_; 23 | rclcpp::TimerBase::SharedPtr timer_; 24 | 25 | void timer_callback() { 26 | // The callback function creates a Twist message with a linear velocity, 27 | // which will make the turtle move in a straight line, and publishes the message. 28 | geometry_msgs::msg::Twist twist; 29 | twist.linear.x = 2.0; 30 | twist.angular.z = 0.0; 31 | cmd_vel_pub_->publish(twist); 32 | } 33 | }; 34 | 35 | int main(int argc, char **argv) { 36 | // Initialize ROS, create the TurtlesimStraightLine node, and spin it 37 | rclcpp::init(argc, argv); 38 | rclcpp::spin(std::make_shared()); 39 | rclcpp::shutdown(); 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /drive_turtle/src/p1_b_multi_straight_drive.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Author: Muhammad Luqman 3 | * Organization: Robotisim 4 | * 5 | * This ROS2 node, "turtlesim_straight_line", publishes geometry_msgs/Twist messages 6 | * at a regular interval to make two Turtlebot3 robots move in a straight line. 7 | * 8 | * This node publishes to the "/robot1/turtle1/cmd_vel" and "/robot2/turtle1/cmd_vel" topics. 9 | */ 10 | 11 | 12 | #include "rclcpp/rclcpp.hpp" 13 | #include "geometry_msgs/msg/twist.hpp" 14 | 15 | class TurtlesimStraightLine : public rclcpp::Node { 16 | public: 17 | TurtlesimStraightLine() : Node("turtlesim_straight_line") { 18 | cmd_vel_pub_1_ = this->create_publisher("/robot1/turtle1/cmd_vel", 10); 19 | cmd_vel_pub_2_ = this->create_publisher("/robot2/turtle1/cmd_vel", 10); 20 | timer_ = this->create_wall_timer(std::chrono::milliseconds(100), 21 | std::bind(&TurtlesimStraightLine::timer_callback, this)); 22 | } 23 | 24 | private: 25 | rclcpp::Publisher::SharedPtr cmd_vel_pub_1_; 26 | rclcpp::Publisher::SharedPtr cmd_vel_pub_2_; 27 | rclcpp::TimerBase::SharedPtr timer_; 28 | 29 | void timer_callback() { 30 | // Publish the Twist message for both robots to move in a straight line 31 | 32 | geometry_msgs::msg::Twist twist; 33 | twist.linear.x = 2.0; 34 | twist.angular.z = 0.0; 35 | 36 | cmd_vel_pub_1_->publish(twist); 37 | cmd_vel_pub_2_->publish(twist); 38 | } 39 | }; 40 | 41 | int main(int argc, char **argv) { 42 | rclcpp::init(argc, argv); 43 | rclcpp::spin(std::make_shared()); 44 | rclcpp::shutdown(); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /drive_turtle/src/p1_c_multi_turtle_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Author: Muhammad Luqman 3 | * Organization: Robotisim 4 | * 5 | * This ROS2 node, "turtlesim_straight_line", is a general-purpose controller for a Turtlebot3 robot. 6 | * It takes the command velocity topic and the linear velocity as parameters, 7 | * and publishes geometry_msgs/Twist messages to make the robot move. 8 | * 9 | * The node can be customized for different robots by changing the parameters. 10 | */ 11 | #include "rclcpp/rclcpp.hpp" 12 | #include "geometry_msgs/msg/twist.hpp" 13 | 14 | class TurtlesimStraightLine : public rclcpp::Node { 15 | public: 16 | TurtlesimStraightLine() : Node("turtlesim_straight_line") { 17 | this->declare_parameter("cmd_vel_topic", "/turtle1/cmd_vel"); 18 | this->declare_parameter("linear_velocity", 2.0); 19 | 20 | std::string cmd_vel_topic = this->get_parameter("cmd_vel_topic").as_string(); 21 | linear_velocity_ = this->get_parameter("linear_velocity").as_double(); 22 | 23 | cmd_vel_pub_ = this->create_publisher(cmd_vel_topic, 10); 24 | timer_ = this->create_wall_timer(std::chrono::milliseconds(100), 25 | std::bind(&TurtlesimStraightLine::timer_callback, this)); 26 | } 27 | 28 | private: 29 | rclcpp::Publisher::SharedPtr cmd_vel_pub_; 30 | rclcpp::TimerBase::SharedPtr timer_; 31 | double linear_velocity_; 32 | 33 | void timer_callback() { 34 | geometry_msgs::msg::Twist twist; 35 | twist.linear.x = linear_velocity_; 36 | twist.angular.z = 0.0; 37 | cmd_vel_pub_->publish(twist); 38 | } 39 | }; 40 | 41 | int main(int argc, char **argv) { 42 | rclcpp::init(argc, argv); 43 | rclcpp::spin(std::make_shared()); 44 | rclcpp::shutdown(); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /microROS_bot/.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | -------------------------------------------------------------------------------- /microROS_bot/.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 3 | // for the documentation about the extensions.json format 4 | "recommendations": [ 5 | "platformio.platformio-ide" 6 | ], 7 | "unwantedRecommendations": [ 8 | "ms-vscode.cpptools-extension-pack" 9 | ] 10 | } 11 | -------------------------------------------------------------------------------- /microROS_bot/Readme.md: -------------------------------------------------------------------------------- 1 | - Project [#4](https://github.com/Robotisim/mobile_robotics_ROS2/issues/23): MicroROS Controlled Differential Drive Bot -> [Running Guide](https://github.com/Robotisim/mobile_robotics_ROS2/wiki/Project-%234:-MicroROS-Controlled-Differential-Drive-Bot) 2 | -------------------------------------------------------------------------------- /microROS_bot/include/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project header files. 3 | 4 | A header file is a file containing C declarations and macro definitions 5 | to be shared between several project source files. You request the use of a 6 | header file in your project source file (C, C++, etc) located in `src` folder 7 | by including it, with the C preprocessing directive `#include'. 8 | 9 | ```src/main.c 10 | 11 | #include "header.h" 12 | 13 | int main (void) 14 | { 15 | ... 16 | } 17 | ``` 18 | 19 | Including a header file produces the same results as copying the header file 20 | into each source file that needs it. Such copying would be time-consuming 21 | and error-prone. With a header file, the related declarations appear 22 | in only one place. If they need to be changed, they can be changed in one 23 | place, and programs that include the header file will automatically use the 24 | new version when next recompiled. The header file eliminates the labor of 25 | finding and changing all the copies as well as the risk that a failure to 26 | find one copy will result in inconsistencies within a program. 27 | 28 | In C, the usual convention is to give header files names that end with `.h'. 29 | It is most portable to use only letters, digits, dashes, and underscores in 30 | header file names, and at most one dot. 31 | 32 | Read more about using header files in official GCC documentation: 33 | 34 | * Include Syntax 35 | * Include Operation 36 | * Once-Only Headers 37 | * Computed Includes 38 | 39 | https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html 40 | -------------------------------------------------------------------------------- /microROS_bot/lib/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for project specific (private) libraries. 3 | PlatformIO will compile them to static libraries and link into executable file. 4 | 5 | The source code of each library should be placed in a an own separate directory 6 | ("lib/your_library_name/[here are source files]"). 7 | 8 | For example, see a structure of the following two libraries `Foo` and `Bar`: 9 | 10 | |--lib 11 | | | 12 | | |--Bar 13 | | | |--docs 14 | | | |--examples 15 | | | |--src 16 | | | |- Bar.c 17 | | | |- Bar.h 18 | | | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html 19 | | | 20 | | |--Foo 21 | | | |- Foo.c 22 | | | |- Foo.h 23 | | | 24 | | |- README --> THIS FILE 25 | | 26 | |- platformio.ini 27 | |--src 28 | |- main.c 29 | 30 | and a contents of `src/main.c`: 31 | ``` 32 | #include 33 | #include 34 | 35 | int main (void) 36 | { 37 | ... 38 | } 39 | 40 | ``` 41 | 42 | PlatformIO Library Dependency Finder will find automatically dependent 43 | libraries scanning project source files. 44 | 45 | More information about PlatformIO Library Dependency Finder 46 | - https://docs.platformio.org/page/librarymanager/ldf.html 47 | -------------------------------------------------------------------------------- /microROS_bot/lib/motor_control/motor_control.cpp: -------------------------------------------------------------------------------- 1 | #include "motor_control.h" 2 | int l_pwm , r_pwm,integral_error,base_pwm=120; 3 | float kp=10,ki=0.5,total_error,proportional_error; 4 | 5 | void motor_setup(){ 6 | ledcSetup(pwm_channel_mr_a , 5000, 8 ); //2^8 = 256 , 0-255 7 | ledcSetup(pwm_channel_mr_b , 5000, 8 ); //2^8 = 256 , 0-255 8 | ledcSetup(pwm_channel_ml_a , 5000, 8 ); //2^8 = 256 , 0-255 9 | ledcSetup(pwm_channel_ml_b , 5000, 8 ); //2^8 = 256 , 0-255 10 | 11 | 12 | ledcAttachPin(motor_right_a,pwm_channel_mr_a); 13 | ledcAttachPin(motor_right_b,pwm_channel_mr_b); 14 | ledcAttachPin(motor_left_a,pwm_channel_ml_a); 15 | ledcAttachPin(motor_left_b,pwm_channel_ml_b); 16 | } 17 | 18 | void forward(int speed) { 19 | ledcWrite(pwm_channel_mr_a, speed); 20 | ledcWrite(pwm_channel_ml_a, speed); 21 | ledcWrite(pwm_channel_mr_b, 0); 22 | ledcWrite(pwm_channel_ml_b, 0); 23 | } 24 | 25 | void reverse(int speed) { 26 | ledcWrite(pwm_channel_mr_b, speed); 27 | ledcWrite(pwm_channel_ml_b, speed); 28 | ledcWrite(pwm_channel_mr_a, 0); 29 | ledcWrite(pwm_channel_ml_a, 0); 30 | } 31 | 32 | void stop() { 33 | ledcWrite(pwm_channel_mr_a, 0); 34 | ledcWrite(pwm_channel_ml_a, 0); 35 | ledcWrite(pwm_channel_mr_b, 0); 36 | ledcWrite(pwm_channel_ml_b, 0); 37 | } 38 | 39 | void right(int speed) { 40 | ledcWrite(pwm_channel_mr_a, 0); 41 | ledcWrite(pwm_channel_ml_a, speed); 42 | ledcWrite(pwm_channel_mr_b, 0); 43 | ledcWrite(pwm_channel_ml_b, 0); 44 | } 45 | 46 | void left(int speed) { 47 | ledcWrite(pwm_channel_mr_a, speed); 48 | ledcWrite(pwm_channel_ml_a, 0); 49 | ledcWrite(pwm_channel_mr_b, 0); 50 | ledcWrite(pwm_channel_ml_b, 0); 51 | } 52 | 53 | std::pair error_motor_drive(int error){ 54 | // Dynamic Speed Adjustment 55 | int dynamic_base_pwm = base_pwm; 56 | if(abs(error) == 0) { // For faster straight Line Movement 57 | dynamic_base_pwm = base_pwm + 120; 58 | } 59 | 60 | integral_error = constrain(integral_error + error, -100, 100); 61 | integral_error = ki * integral_error; 62 | proportional_error = kp * error; 63 | total_error = proportional_error + integral_error; 64 | 65 | l_pwm = dynamic_base_pwm - total_error; 66 | r_pwm = dynamic_base_pwm + total_error; 67 | 68 | l_pwm = constrain(l_pwm+5, 70, 255); 69 | r_pwm = constrain(r_pwm, 70, 255); 70 | 71 | ledcWrite(pwm_channel_mr_a, r_pwm); 72 | ledcWrite(pwm_channel_ml_a, l_pwm); 73 | ledcWrite(pwm_channel_mr_b, 0); 74 | ledcWrite(pwm_channel_ml_b, 0); 75 | 76 | return std::make_pair(l_pwm, r_pwm); 77 | } 78 | -------------------------------------------------------------------------------- /microROS_bot/lib/motor_control/motor_control.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTOR_CONTROL_H 2 | #define MOTOR_CONTROL_H 3 | 4 | #include 5 | 6 | // Pin Definitions 7 | #define motor_right_b 12 8 | #define motor_right_a 13 9 | #define motor_left_a 14 10 | #define motor_left_b 27 11 | 12 | // Channels 13 | #define pwm_channel_mr_a 0 14 | #define pwm_channel_mr_b 1 15 | #define pwm_channel_ml_a 2 16 | #define pwm_channel_ml_b 3 17 | 18 | // Functions 19 | void forward(int speed); 20 | void stop(); 21 | void left(int speed); 22 | void right(int speed); 23 | void reverse(int speed); 24 | void motor_setup(); 25 | std::pair error_motor_drive(int error); 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /microROS_bot/lib/oled_display/oled_display.cpp: -------------------------------------------------------------------------------- 1 | #include "oled_display.h" 2 | 3 | 4 | void setupDisplay(Adafruit_SSD1306 &display){ 5 | display.begin(SSD1306_SWITCHCAPVCC, 0x3C); 6 | display.clearDisplay(); 7 | display.setTextSize(2); 8 | display.setCursor(10,10); 9 | display.setTextColor(SSD1306_WHITE); 10 | display.print("MicroROS"); 11 | display.drawRect(5,9,120,20,WHITE); 12 | display.display(); 13 | delay(2000); 14 | } 15 | 16 | void displayLineFollowing(Adafruit_SSD1306 &display,int &lpwm,int &rpwm){ 17 | display.clearDisplay(); 18 | display.setTextSize(2); 19 | display.setCursor(0,0); 20 | display.setTextColor(SSD1306_WHITE); 21 | 22 | 23 | display.print("lpwm :");display.println(lpwm); 24 | display.print("rpwm :");display.println(rpwm); 25 | display.display(); 26 | } 27 | 28 | 29 | 30 | void drawRightArrow(Adafruit_SSD1306 &display) { 31 | display.clearDisplay(); 32 | 33 | 34 | for (int i = -1; i <= 1; i++) { 35 | display.drawLine(30, 15 + i, 80, 15 + i, WHITE); 36 | } 37 | // Upper diagonal 38 | for (int i = -1; i <= 1; i++) { 39 | for (int j = -1; j <= 1; j++) { 40 | display.drawLine(80, 15, 60 + i, 10 + j, WHITE); 41 | } 42 | } 43 | // Lower diagonal 44 | for (int i = -1; i <= 1; i++) { 45 | for (int j = -1; j <= 1; j++) { 46 | display.drawLine(80, 15, 60 + i, 20 + j, WHITE); 47 | } 48 | }display.display(); 49 | } 50 | void drawLeftArrow(Adafruit_SSD1306 &display) { 51 | display.clearDisplay(); 52 | 53 | for (int i = -1; i <= 1; i++) { 54 | display.drawLine(80, 15 + i, 30, 15 + i, WHITE); 55 | } 56 | // Upper diagonal 57 | for (int i = -1; i <= 1; i++) { 58 | for (int j = -1; j <= 1; j++) { 59 | display.drawLine(30, 15, 50 + i, 10 + j, WHITE); 60 | } 61 | } 62 | // Lower diagonal 63 | for (int i = -1; i <= 1; i++) { 64 | for (int j = -1; j <= 1; j++) { 65 | display.drawLine(30, 15, 50 + i, 20 + j, WHITE); 66 | } 67 | } 68 | display.display(); 69 | } 70 | void drawForwardArrow(Adafruit_SSD1306 &display) { 71 | display.clearDisplay(); 72 | 73 | // up 74 | for (int i = -1; i <= 1; i++) { 75 | display.drawLine(55 + i, 5, 55 + i, 25, WHITE); 76 | } 77 | // Left diagonal 78 | for (int i = -1; i <= 1; i++) { 79 | for (int j = -1; j <= 1; j++) { 80 | display.drawLine(45, 15, 55 + i, 5 + j, WHITE); 81 | } 82 | } 83 | // Right diagonal 84 | for (int i = -1; i <= 1; i++) { 85 | for (int j = -1; j <= 1; j++) { 86 | display.drawLine(65, 15, 55 + i, 5 + j, WHITE); 87 | } 88 | }display.display(); 89 | } 90 | void drawBackwardArrow(Adafruit_SSD1306 &display) { 91 | display.clearDisplay(); 92 | 93 | 94 | // Vertical line 95 | for (int i = -1; i <= 1; i++) { 96 | display.drawLine(55 + i, 5, 55 + i, 20, WHITE); 97 | } 98 | // Left diagonal 99 | for (int i = -1; i <= 1; i++) { 100 | for (int j = -1; j <= 1; j++) { 101 | display.drawLine(45, 15, 55 + i, 20 + j, WHITE); 102 | } 103 | } 104 | // Right diagonal 105 | for (int i = -1; i <= 1; i++) { 106 | for (int j = -1; j <= 1; j++) { 107 | display.drawLine(65, 15, 55 + i, 20 + j, WHITE); 108 | } 109 | }display.display(); 110 | } 111 | void drawStopCircle(Adafruit_SSD1306 &display) { 112 | display.clearDisplay(); 113 | 114 | for (int i = -1; i <= 1; i++) { 115 | display.drawCircle(64, 15, 10 + i, WHITE); 116 | }display.display(); 117 | } 118 | -------------------------------------------------------------------------------- /microROS_bot/lib/oled_display/oled_display.h: -------------------------------------------------------------------------------- 1 | #ifndef OLED_DISPLAY_H 2 | #define OLED_DISPLAY_H 3 | 4 | #include 5 | #include 6 | 7 | 8 | void drawRightArrow(Adafruit_SSD1306 &display); 9 | void drawLeftArrow(Adafruit_SSD1306 &display); 10 | void drawForwardArrow(Adafruit_SSD1306 &display); 11 | void drawBackwardArrow(Adafruit_SSD1306 &display); 12 | void drawStopCircle(Adafruit_SSD1306 &display); 13 | 14 | void setupDisplay(Adafruit_SSD1306 &display); 15 | void displayLineFollowing(Adafruit_SSD1306 &display,int &lpwm,int &rpwm); 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /microROS_bot/lib/ros_communication/ros_communication.cpp: -------------------------------------------------------------------------------- 1 | #include "ros_communication.h" 2 | #include "motor_control.h" 3 | #include "oled_display.h" 4 | 5 | rcl_subscription_t cmd_vel_sub; 6 | geometry_msgs__msg__Twist cmd_vel_msg; 7 | 8 | rclc_executor_t executor; 9 | rclc_support_t support; 10 | rcl_allocator_t allocator; 11 | rcl_node_t node; 12 | 13 | Adafruit_SSD1306 display; 14 | 15 | RosCommunication::RosCommunication(){ 16 | } 17 | 18 | void RosCommunication::initialize(){ 19 | Serial.begin(115200); 20 | Serial.println("ROS Communication node started"); 21 | setupDisplay(display); 22 | 23 | 24 | 25 | // Adding Wifi 26 | IPAddress agent_ip(192, 168, 100, 21); 27 | size_t agent_port = 8888; 28 | 29 | char ssid[] = "Jhelum.net [Luqman House]"; 30 | char psk[]= "7861234786"; 31 | 32 | set_microros_wifi_transports(ssid, psk, agent_ip, agent_port); 33 | // set_microros_serial_transports(Serial); 34 | 35 | delay(2000); 36 | 37 | allocator = rcl_get_default_allocator(); 38 | rclc_support_init(&support, 0, NULL, &allocator); 39 | rclc_node_init_default(&node, "cmd_vel_sub", "", &support); 40 | } 41 | 42 | 43 | void RosCommunication::executors_start(){ 44 | rclc_executor_init(&executor, &support.context, 1, &allocator); 45 | rclc_executor_add_subscription(&executor, &cmd_vel_sub, &cmd_vel_msg,&RosCommunication::cmd_vel_callback, ON_NEW_DATA); 46 | 47 | Serial.println("Executors Started"); 48 | } 49 | void RosCommunication::subscriber_define(){ 50 | 51 | rclc_subscription_init_default( 52 | &cmd_vel_sub, 53 | &node, 54 | ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), 55 | "/cmd_vel"); 56 | 57 | } 58 | 59 | void RosCommunication::cmd_vel_callback(const void *msg_recv){ 60 | const geometry_msgs__msg__Twist * recieved_data = (const geometry_msgs__msg__Twist *) msg_recv ; 61 | float linear_vel = recieved_data->linear.x; 62 | float angular_vel = recieved_data->angular.z; 63 | 64 | Serial.print(linear_vel);Serial.print(" / ");Serial.println(angular_vel); 65 | 66 | if(linear_vel > 0) { 67 | Serial.println("Forward"); 68 | forward(linear_vel * 255); 69 | drawForwardArrow(display); 70 | } else if(linear_vel < 0) { 71 | Serial.println("Reverse"); 72 | reverse(-linear_vel * 255); 73 | drawBackwardArrow(display); 74 | } else if(angular_vel < 0) { 75 | Serial.println("right"); 76 | right(-angular_vel *255); 77 | drawRightArrow(display); 78 | } else if(angular_vel > 0) { 79 | Serial.println("left"); 80 | drawLeftArrow(display); 81 | left(angular_vel *255); 82 | } else { 83 | Serial.println("Stop"); 84 | stop(); 85 | drawStopCircle(display); 86 | } 87 | 88 | 89 | 90 | 91 | } 92 | void RosCommunication::start_receiving_msgs(){ 93 | rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); 94 | delay(100); 95 | } -------------------------------------------------------------------------------- /microROS_bot/lib/ros_communication/ros_communication.h: -------------------------------------------------------------------------------- 1 | #ifndef ROS_COMMUNICATION_H 2 | #define ROS_COMMUNICATION_H 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | class RosCommunication{ 12 | public: 13 | RosCommunication(); 14 | void initialize(); 15 | void subscriber_define(); 16 | static void cmd_vel_callback(const void *msg_recv); 17 | void start_receiving_msgs(); 18 | void executors_start(); 19 | 20 | 21 | private: 22 | 23 | static float linear_vel; 24 | static float angular_vel; 25 | 26 | }; 27 | 28 | 29 | #endif -------------------------------------------------------------------------------- /microROS_bot/platformio.ini: -------------------------------------------------------------------------------- 1 | ; PlatformIO Project Configuration File 2 | ; 3 | ; Build options: build flags, source filter 4 | ; Upload options: custom upload port, speed and extra flags 5 | ; Library options: dependencies, extra library storages 6 | ; Advanced options: extra scripting 7 | ; 8 | ; Please visit documentation for the other options and examples 9 | ; https://docs.platformio.org/page/projectconf.html 10 | 11 | [env:esp32doit-devkit-v1] 12 | platform = espressif32 13 | board = esp32doit-devkit-v1 14 | framework = arduino 15 | 16 | monitor_speed = 115200 17 | monitor_rts = 0 18 | monitor_dtr = 0 19 | monitor_port = /dev/ttyUSB0 20 | 21 | board_microros_distro = humble 22 | board_microros_transport = wifi 23 | lib_deps = 24 | https://github.com/micro-ROS/micro_ros_platformio 25 | adafruit/Adafruit SSD1306@^2.5.7 26 | 27 | -------------------------------------------------------------------------------- /microROS_bot/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ros_communication.h" 3 | #include "motor_control.h" 4 | RosCommunication ros_communication ; 5 | 6 | 7 | void setup() { 8 | motor_setup(); 9 | ros_communication.initialize(); 10 | ros_communication.subscriber_define(); 11 | ros_communication.executors_start(); 12 | } 13 | 14 | void loop() { 15 | ros_communication.start_receiving_msgs(); 16 | 17 | } -------------------------------------------------------------------------------- /microROS_bot/test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PlatformIO Test Runner and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PlatformIO Unit Testing: 11 | - https://docs.platformio.org/en/latest/advanced/unit-testing/index.html 12 | -------------------------------------------------------------------------------- /point_cloud_processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(point_cloud_processing) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(PCL REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | find_package(sensor_msgs REQUIRED) 13 | find_package(pcl_conversions REQUIRED) 14 | find_package(visualization_msgs REQUIRED) 15 | include_directories(${PCL_INCLUDE_DIRS}) 16 | link_directories(${PCL_LIBRARY_DIRS}) 17 | add_definitions(${PCL_DEFINITIONS}) 18 | 19 | 20 | 21 | include_directories(${PCL_INCLUDE_DIRS}) 22 | link_directories(${PCL_LIBRARY_DIRS}) 23 | add_definitions(${PCL_DEFINITIONS}) 24 | 25 | 26 | add_executable(p9_a_pcd_import src/p9_a_table_scene.cpp) 27 | target_link_libraries(p9_a_pcd_import ${PCL_LIBRARIES}) 28 | 29 | add_executable(p9_b_plane_cloud src/p9_b_planner_cloud.cpp) 30 | target_link_libraries(p9_b_plane_cloud ${PCL_LIBRARIES}) 31 | 32 | add_executable(p9_c_circular_cloud src/p9_c_circular_cloud.cpp) 33 | target_link_libraries(p9_c_circular_cloud ${PCL_LIBRARIES}) 34 | 35 | 36 | add_executable(p9_d_filtering_segmentation src/p9_d_filtering_segmentation.cpp) 37 | target_link_libraries(p9_d_filtering_segmentation ${PCL_LIBRARIES}) 38 | 39 | add_executable(p9_e_path_creation src/p9_e_point_path_drawing.cpp) 40 | target_link_libraries(p9_e_path_creation ${PCL_LIBRARIES}) 41 | 42 | 43 | 44 | 45 | install(TARGETS p9_a_pcd_import p9_b_plane_cloud p9_c_circular_cloud p9_d_filtering_segmentation p9_e_path_creation 46 | DESTINATION lib/${PROJECT_NAME}) 47 | 48 | install(DIRECTORY launch 49 | DESTINATION share/${PROJECT_NAME}/ 50 | ) 51 | 52 | ament_package() 53 | -------------------------------------------------------------------------------- /point_cloud_processing/launch/p9_a_rtab_mapping.launch.py: -------------------------------------------------------------------------------- 1 | # Requirements: 2 | # Install Turtlebot3 packages 3 | # Modify turtlebot3_waffle SDF: 4 | # 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf 5 | # 2) Add 6 | # 7 | # camera_rgb_frame 8 | # camera_rgb_optical_frame 9 | # 0 0 0 -1.57079632679 0 -1.57079632679 10 | # 11 | # 0 0 1 12 | # 13 | # 14 | # 3) Rename to 15 | # 4) Add 16 | # 5) Change to 17 | # 6) Change image width/height from 1920x1080 to 640x480 18 | # 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans 19 | # hitting the robot itself 20 | # Example: 21 | # $ export TURTLEBOT3_MODEL=waffle 22 | # $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py 23 | # 24 | # SLAM: 25 | # $ ros2 launch rtabmap_demos turtlebot3_rgbd.launch.py 26 | # OR 27 | # $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info approx_sync:=true qos:=2 28 | # $ ros2 run topic_tools relay /rtabmap/map /map 29 | # 30 | # Navigation (install nav2_bringup package): 31 | # $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True 32 | # $ ros2 launch nav2_bringup rviz_launch.py 33 | # 34 | # Teleop: 35 | # $ ros2 run turtlebot3_teleop teleop_keyboard 36 | 37 | from launch import LaunchDescription 38 | from launch.actions import DeclareLaunchArgument 39 | from launch.substitutions import LaunchConfiguration 40 | from launch.conditions import IfCondition, UnlessCondition 41 | from launch_ros.actions import Node 42 | from launch.actions import IncludeLaunchDescription 43 | from launch.launch_description_sources import PythonLaunchDescriptionSource 44 | from ament_index_python.packages import get_package_share_directory 45 | import os 46 | def generate_launch_description(): 47 | 48 | use_sim_time = LaunchConfiguration('use_sim_time') 49 | qos = LaunchConfiguration('qos') 50 | localization = LaunchConfiguration('localization') 51 | 52 | parameters={ 53 | 'frame_id':'base_footprint', 54 | 'use_sim_time':use_sim_time, 55 | 'subscribe_depth':True, 56 | 'use_action_for_goal':True, 57 | 'qos_image':qos, 58 | 'qos_imu':qos, 59 | 'Reg/Force3DoF':'true', 60 | 'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D) 61 | } 62 | 63 | remappings=[ 64 | ('rgb/image', '/camera/image_raw'), 65 | ('rgb/camera_info', '/camera/camera_info'), 66 | ('depth/image', '/camera/depth/image_raw')] 67 | 68 | return LaunchDescription([ 69 | 70 | # Launch arguments 71 | DeclareLaunchArgument( 72 | 'use_sim_time', default_value='true', 73 | description='Use simulation (Gazebo) clock if true'), 74 | 75 | DeclareLaunchArgument( 76 | 'qos', default_value='2', 77 | description='QoS used for input sensor topics'), 78 | 79 | DeclareLaunchArgument( 80 | 'localization', default_value='false', 81 | description='Launch in localization mode.'), 82 | 83 | # Nodes to launch 84 | # IncludeLaunchDescription( 85 | # PythonLaunchDescriptionSource( 86 | # os.path.join(get_package_share_directory('turtlebot3_gazebo') 87 | # , 'launch', 'turtlebot3_world.launch.py') 88 | # ) 89 | # ), 90 | # SLAM mode: 91 | Node( 92 | condition=UnlessCondition(localization), 93 | package='rtabmap_slam', executable='rtabmap', output='screen', 94 | parameters=[parameters], 95 | remappings=remappings, 96 | arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db) 97 | 98 | # Localization mode: 99 | Node( 100 | condition=IfCondition(localization), 101 | package='rtabmap_slam', executable='rtabmap', output='screen', 102 | parameters=[parameters, 103 | {'Mem/IncrementalMemory':'False', 104 | 'Mem/InitWMWithAllNodes':'True'}], 105 | remappings=remappings), 106 | 107 | Node( 108 | package='rtabmap_viz', executable='rtabmap_viz', output='screen', 109 | parameters=[parameters], 110 | remappings=remappings), 111 | ]) -------------------------------------------------------------------------------- /point_cloud_processing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | point_cloud_processing 5 | 0.0.0 6 | TODO: Package description 7 | luqman 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /point_cloud_processing/point_clouds/circular_cloud.pcd: -------------------------------------------------------------------------------- 1 | # .PCD v0.7 - Point Cloud Data file format 2 | VERSION 0.7 3 | FIELDS x y z rgb 4 | SIZE 4 4 4 4 5 | TYPE F F F U 6 | COUNT 1 1 1 1 7 | WIDTH 50 8 | HEIGHT 1 9 | VIEWPOINT 0 0 0 1 0 0 0 10 | POINTS 50 11 | DATA ascii 12 | 3 0 1 4294901760 13 | 2.9763441 0.37599969 1 4294713313 14 | 2.9057496 0.74606967 1 4294328257 15 | 2.7893295 1.1043737 1 4293746083 16 | 2.6289201 1.445261 1 4292835974 17 | 2.4270511 1.7633557 1 4291728747 18 | 2.1869059 2.0536413 1 4290358866 19 | 1.912272 2.3115396 1 4288857148 20 | 1.6074804 2.5329838 1 4287158057 21 | 1.2773379 2.7144811 1 4285326874 22 | 0.92705101 2.8531694 1 4283363854 23 | 0.56214392 2.9468617 1 4281334278 24 | 0.18837155 2.9940801 1 4279303682 25 | -0.18837155 2.9940801 1 4293983746 26 | -0.56214392 2.9468617 1 4291951110 27 | -0.92705101 2.8531694 1 4289917454 28 | -1.2773379 2.7144811 1 4287948314 29 | -1.6074804 2.5329838 1 4286109481 30 | -1.912272 2.3115396 1 4284400700 31 | -2.1869059 2.0536413 1 4282887762 32 | -2.4270511 1.7633557 1 4281505131 33 | -2.6289201 1.445261 1 4280384134 34 | -2.7893295 1.1043737 1 4279459235 35 | -2.9057496 0.74606967 1 4278861761 36 | -2.9763441 0.37599969 1 4278460385 37 | -3 -9.6487355e-16 1 4278255616 38 | -2.9763441 -0.37599969 1 4278509855 39 | -2.9057496 -0.74606967 1 4278894911 40 | -2.7893295 -1.1043737 1 4279477085 41 | -2.6289201 -1.445261 1 4280387194 42 | -2.4270511 -1.7633557 1 4281494421 43 | -2.1869059 -2.0536413 1 4282864302 44 | -1.912272 -2.3115396 1 4284366020 45 | -1.6074804 -2.5329838 1 4286065111 46 | -1.2773379 -2.7144811 1 4287896294 47 | -0.92705101 -2.8531694 1 4289859314 48 | -0.56214392 -2.9468617 1 4291888890 49 | -0.18837155 -2.9940801 1 4293919486 50 | 0.18837155 -2.9940801 1 4279239422 51 | 0.56214392 -2.9468617 1 4281272058 52 | 0.92705101 -2.8531694 1 4283305714 53 | 1.2773379 -2.7144811 1 4285274854 54 | 1.6074804 -2.5329838 1 4287113687 55 | 1.912272 -2.3115396 1 4288822468 56 | 2.1869059 -2.0536413 1 4290335406 57 | 2.4270511 -1.7633557 1 4291718037 58 | 2.6289201 -1.445261 1 4292839034 59 | 2.7893295 -1.1043737 1 4293763933 60 | 2.9057496 -0.74606967 1 4294361407 61 | 2.9763441 -0.37599969 1 4294762783 62 | -------------------------------------------------------------------------------- /point_cloud_processing/point_clouds/plane_.pcd: -------------------------------------------------------------------------------- 1 | # .PCD v0.7 - Point Cloud Data file format 2 | VERSION 0.7 3 | FIELDS x y z 4 | SIZE 4 4 4 5 | TYPE F F F 6 | COUNT 1 1 1 7 | WIDTH 5 8 | HEIGHT 1 9 | VIEWPOINT 0 0 0 1 0 0 0 10 | POINTS 5 11 | DATA ascii 12 | 1 2 3 13 | 4 5 6 14 | 7 8 9 15 | 10 11 12 16 | 12 14 -15 17 | -------------------------------------------------------------------------------- /point_cloud_processing/point_clouds/tb3_world.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Robotisim/mobile_robotics_ROS2/568f7d790ce31b4664e7774458d1f43ae099fd72/point_cloud_processing/point_clouds/tb3_world.pcd -------------------------------------------------------------------------------- /point_cloud_processing/resource/.38553d294371dbd5752426cc805039aeae63dd03.cam: -------------------------------------------------------------------------------- 1 | 30.3091,41.2238/-0.205203,0.354679,1.05885/-13.6375,-12.3666,30.8919/-0.264056,0.924388,0.275283/0.314159/1920,540/10,45 2 | -------------------------------------------------------------------------------- /point_cloud_processing/resource/output.pcd: -------------------------------------------------------------------------------- 1 | # .PCD v0.7 - Point Cloud Data file format 2 | VERSION 0.7 3 | FIELDS x y z rgb 4 | SIZE 4 4 4 4 5 | TYPE F F F U 6 | COUNT 1 1 1 1 7 | WIDTH 50 8 | HEIGHT 1 9 | VIEWPOINT 0 0 0 1 0 0 0 10 | POINTS 50 11 | DATA ascii 12 | 3 0 1 4294901760 13 | 2.9763441 0.37599969 1 4294713313 14 | 2.9057496 0.74606967 1 4294328257 15 | 2.7893295 1.1043737 1 4293746083 16 | 2.6289201 1.445261 1 4292835974 17 | 2.4270511 1.7633557 1 4291728747 18 | 2.1869059 2.0536413 1 4290358866 19 | 1.912272 2.3115396 1 4288857148 20 | 1.6074804 2.5329838 1 4287158057 21 | 1.2773379 2.7144811 1 4285326874 22 | 0.92705101 2.8531694 1 4283363854 23 | 0.56214392 2.9468617 1 4281334278 24 | 0.18837155 2.9940801 1 4279303682 25 | -0.18837155 2.9940801 1 4293983746 26 | -0.56214392 2.9468617 1 4291951110 27 | -0.92705101 2.8531694 1 4289917454 28 | -1.2773379 2.7144811 1 4287948314 29 | -1.6074804 2.5329838 1 4286109481 30 | -1.912272 2.3115396 1 4284400700 31 | -2.1869059 2.0536413 1 4282887762 32 | -2.4270511 1.7633557 1 4281505131 33 | -2.6289201 1.445261 1 4280384134 34 | -2.7893295 1.1043737 1 4279459235 35 | -2.9057496 0.74606967 1 4278861761 36 | -2.9763441 0.37599969 1 4278460385 37 | -3 -9.6487355e-16 1 4278255616 38 | -2.9763441 -0.37599969 1 4278509855 39 | -2.9057496 -0.74606967 1 4278894911 40 | -2.7893295 -1.1043737 1 4279477085 41 | -2.6289201 -1.445261 1 4280387194 42 | -2.4270511 -1.7633557 1 4281494421 43 | -2.1869059 -2.0536413 1 4282864302 44 | -1.912272 -2.3115396 1 4284366020 45 | -1.6074804 -2.5329838 1 4286065111 46 | -1.2773379 -2.7144811 1 4287896294 47 | -0.92705101 -2.8531694 1 4289859314 48 | -0.56214392 -2.9468617 1 4291888890 49 | -0.18837155 -2.9940801 1 4293919486 50 | 0.18837155 -2.9940801 1 4279239422 51 | 0.56214392 -2.9468617 1 4281272058 52 | 0.92705101 -2.8531694 1 4283305714 53 | 1.2773379 -2.7144811 1 4285274854 54 | 1.6074804 -2.5329838 1 4287113687 55 | 1.912272 -2.3115396 1 4288822468 56 | 2.1869059 -2.0536413 1 4290335406 57 | 2.4270511 -1.7633557 1 4291718037 58 | 2.6289201 -1.445261 1 4292839034 59 | 2.7893295 -1.1043737 1 4293763933 60 | 2.9057496 -0.74606967 1 4294361407 61 | 2.9763441 -0.37599969 1 4294762783 62 | -------------------------------------------------------------------------------- /point_cloud_processing/src/p9_a_table_scene.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main(){ 8 | pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2()); 9 | pcl::PCDReader cloud_reader; 10 | std::string pcd_file_name = "table_scene.pcd"; 11 | std::filesystem::path ros2_ws_path = std::filesystem::current_path(); 12 | std::filesystem::path point_cloud_dir = ros2_ws_path/"src/point_cloud_processing/point_clouds"; 13 | std::string pcd_file_path = point_cloud_dir/pcd_file_name; 14 | 15 | cloud_reader.read(pcd_file_path,*cloud); 16 | 17 | std::cout<<"Number of point "<< cloud->width * cloud->height << std::endl; 18 | 19 | return 0; 20 | } -------------------------------------------------------------------------------- /point_cloud_processing/src/p9_b_planner_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | int main() 6 | { 7 | pcl::PointCloud cloud ; 8 | 9 | cloud.push_back(pcl::PointXYZ(1.0 ,2.0 ,3.0)); 10 | cloud.push_back(pcl::PointXYZ(4.0 ,5.0 ,6.0)); 11 | cloud.push_back(pcl::PointXYZ(7.0 ,8.0 ,9.0)); 12 | cloud.push_back(pcl::PointXYZ(10.0,11.0,12.0)); 13 | cloud.push_back(pcl::PointXYZ(12.0,14.0,-15.0)); 14 | 15 | std::string pcd_file_name = "planez_.pcd"; 16 | std::filesystem::path ros2_ws_path = std::filesystem::current_path(); 17 | std::filesystem::path point_cloud_dir = ros2_ws_path/"src/point_cloud_processing/point_clouds/"; 18 | std::string pcd_file_path = point_cloud_dir/pcd_file_name; 19 | 20 | pcl::io::savePCDFileASCII(pcd_file_path,cloud); 21 | std::cout< 2 | #include 3 | #include 4 | #include 5 | 6 | int main() 7 | { 8 | pcl::PointCloud cloud ; 9 | double radius = 3.0; 10 | int num_points= 50; 11 | double angular_step_size = 2.0* M_PI /num_points; 12 | 13 | for(int i=0;i 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | 26 | typedef pcl::PointXYZ PointT; 27 | 28 | void cloud_saver(const std::string& file_name,std::string& path, pcl::PointCloud::Ptr cloud_arg){ 29 | pcl::PCDWriter cloud_writer; 30 | cloud_writer.write(path+std::string(file_name),*cloud_arg); 31 | } 32 | 33 | int main() 34 | { 35 | // ******************************** Reading the Cloud 36 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud) ; 37 | pcl::PCDReader cloud_reader; 38 | std::string path = "/home/luqman/robotisim_ws/src/point_cloud_processing/point_clouds/"; 39 | std::string input_cloud = "tb3_world.pcd"; 40 | cloud_reader.read(path+input_cloud , *cloud); 41 | 42 | // ******************************** Voxel Filter 43 | 44 | pcl::PointCloud::Ptr voxel_cloud (new pcl::PointCloud) ; 45 | pcl::VoxelGrid voxel_filter; 46 | voxel_filter.setInputCloud(cloud); 47 | voxel_filter.setLeafSize(0.05 , 0.05, 0.05); 48 | voxel_filter.filter(*voxel_cloud); 49 | 50 | // ******************************** Pass through Filter 51 | // Along X Axis 52 | pcl::PointCloud::Ptr passthrough_cloud (new pcl::PointCloud) ; 53 | pcl::PassThrough passing_x; 54 | passing_x.setInputCloud(voxel_cloud); 55 | passing_x.setFilterFieldName("x"); 56 | passing_x.setFilterLimits(-1.7,1.7); 57 | passing_x.filter(*passthrough_cloud); 58 | 59 | // Along Y Axis 60 | pcl::PassThrough passing_y; 61 | passing_y.setInputCloud(passthrough_cloud); 62 | passing_y.setFilterFieldName("y"); 63 | passing_y.setFilterLimits(-1.7,1.7); 64 | passing_y.filter(*passthrough_cloud); 65 | 66 | // ******************************** Planner Segmentation 67 | pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 68 | pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 69 | pcl::PointCloud::Ptr plane_segmented_cloud (new pcl::PointCloud) ; 70 | pcl::SACSegmentation plane_segmentor; 71 | pcl::ExtractIndices indices_extractor; 72 | 73 | plane_segmentor.setInputCloud(passthrough_cloud); 74 | plane_segmentor.setModelType(pcl::SACMODEL_PLANE); 75 | plane_segmentor.setMethodType(pcl::SAC_RANSAC); 76 | plane_segmentor.setDistanceThreshold(0.01); 77 | plane_segmentor.segment(*inliers,*coefficients); 78 | 79 | indices_extractor.setInputCloud(passthrough_cloud); 80 | indices_extractor.setIndices(inliers); 81 | indices_extractor.setNegative(false); 82 | indices_extractor.filter(*plane_segmented_cloud); 83 | cloud_saver("plane.pcd",path,plane_segmented_cloud); 84 | 85 | // ******************************** Cylinder Segmentation 86 | // Normal Extraction Objects 87 | pcl::ModelCoefficients::Ptr cylinder_co (new pcl::ModelCoefficients); 88 | pcl::PointIndices::Ptr cylinder_in (new pcl::PointIndices); 89 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); 90 | pcl::PointCloud::Ptr cloud_normals (new pcl::PointCloud); 91 | pcl::PointCloud::Ptr cylinder_cloud (new pcl::PointCloud ()); 92 | // Normals computation objects 93 | pcl::NormalEstimation normals_estimator; 94 | pcl::SACSegmentationFromNormals cylinder_segmentor; 95 | pcl::ExtractIndices cylinder_indices_extractor; 96 | pcl::ExtractIndices cylinder_normal_indices_extractor; 97 | // Performing estimation of normals 98 | normals_estimator.setSearchMethod(tree); 99 | normals_estimator.setInputCloud(plane_segmented_cloud); 100 | normals_estimator.setKSearch(30); 101 | normals_estimator.compute(*cloud_normals); 102 | 103 | // Parameters for segmentation 104 | cylinder_segmentor.setOptimizeCoefficients(true); 105 | cylinder_segmentor.setModelType(pcl::SACMODEL_CYLINDER); 106 | cylinder_segmentor.setMethodType(pcl::SAC_RANSAC); 107 | cylinder_segmentor.setNormalDistanceWeight(0.5); 108 | cylinder_segmentor.setMaxIterations(10000); 109 | cylinder_segmentor.setDistanceThreshold(0.05); 110 | cylinder_segmentor.setRadiusLimits(0.1, 0.4); 111 | int looping_var=0; 112 | 113 | while(true){ 114 | 115 | 116 | // Appplying segmentation 117 | cylinder_segmentor.setInputCloud(plane_segmented_cloud); 118 | cylinder_segmentor.setInputNormals(cloud_normals); 119 | cylinder_segmentor.segment(*cylinder_in,*cylinder_co); 120 | 121 | // extracting indices 122 | cylinder_indices_extractor.setInputCloud(plane_segmented_cloud); 123 | cylinder_indices_extractor.setIndices(cylinder_in); 124 | cylinder_indices_extractor.setNegative(false); 125 | cylinder_indices_extractor.filter(*cylinder_cloud); 126 | 127 | if(!cylinder_cloud->points.empty()){ 128 | std::stringstream loop_name_cloud; loop_name_cloud <<"cloud_"<points.size()<points.size() > 50){ 131 | cloud_saver(loop_name_cloud.str(),path,cylinder_cloud); 132 | looping_var++; 133 | } 134 | 135 | cylinder_indices_extractor.setNegative(true); 136 | cylinder_indices_extractor.filter(*plane_segmented_cloud); 137 | 138 | // processing normals 139 | cylinder_normal_indices_extractor.setInputCloud(cloud_normals); 140 | cylinder_normal_indices_extractor.setIndices(cylinder_in); 141 | cylinder_normal_indices_extractor.setNegative(true); 142 | cylinder_normal_indices_extractor.filter(*cloud_normals); 143 | 144 | } 145 | else{ 146 | return 0; 147 | } 148 | 149 | 150 | } 151 | return 0; 152 | } -------------------------------------------------------------------------------- /point_cloud_processing/src/p9_e_point_path_drawing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | int main() { 9 | std::string pcd_file_name = "plane.pcd"; 10 | std::filesystem::path ros2_ws_path = std::filesystem::current_path(); 11 | std::filesystem::path point_cloud_dir = ros2_ws_path/"src/point_cloud_processing/point_clouds/"; 12 | std::string pcd_file_path = point_cloud_dir/pcd_file_name; 13 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 14 | pcl::io::loadPCDFile(pcd_file_path, *cloud); 15 | 16 | pcl::visualization::PCLVisualizer viewer("Point Cloud with Path"); 17 | viewer.addPointCloud(cloud,"Groudn Plane "); 18 | float min_x,min_y,max_y =0; 19 | for(int i=0;ipoints.size();i++){ 20 | pcl::PointXYZ point = cloud->points[i]; 21 | if(point.x < min_x){ 22 | min_x=point.x; 23 | } 24 | if(point.y < min_y){ 25 | min_y = point.y; 26 | } 27 | if (point.y > max_y) { 28 | max_y = point.y; 29 | } 30 | } 31 | 32 | float ball_radius = 0.2; 33 | float ball_x = min_x + 1.2; 34 | float ball_y = min_y; 35 | 36 | int i=0; 37 | while(ball_y <= max_y){ 38 | pcl::PointXYZ point; 39 | point.x =ball_x; 40 | point.y=ball_y; 41 | 42 | 43 | viewer.addSphere(point,0.1,0.0,1.0,0.0,"Sphere"+std::to_string(i++)); 44 | ball_y += ball_radius * 1.5; 45 | 46 | } 47 | viewer.spin(); 48 | 49 | 50 | return 0; 51 | } 52 | --------------------------------------------------------------------------------