├── .gitignore ├── .gitmodules ├── LICENSE ├── P1_Simulation ├── .gitignore ├── CMakeLists.txt ├── README.md ├── model │ ├── Building │ │ ├── model.config │ │ └── model.sdf │ └── Robot │ │ ├── model.config │ │ └── model.sdf ├── script │ └── hello.cpp └── world │ └── office ├── P2_Perception ├── .gitignore ├── README.md └── src │ ├── CMakeLists.txt │ ├── ball_chaser │ ├── CMakeLists.txt │ ├── launch │ │ └── ball_chaser.launch │ ├── package.xml │ ├── src │ │ ├── drive_bot.cpp │ │ └── process_image.cpp │ └── srv │ │ └── DriveToTarget.srv │ └── my_robot │ ├── CMakeLists.txt │ ├── launch │ ├── robot_description.launch │ └── world.launch │ ├── meshes │ └── hokuyo.dae │ ├── package.xml │ ├── urdf │ ├── my_robot.gazebo │ └── my_robot.xacro │ └── world │ └── andrews.world ├── P3_Localization ├── .gitignore ├── README.md └── src │ ├── CMakeLists.txt │ ├── ball_chaser │ ├── CMakeLists.txt │ ├── launch │ │ └── ball_chaser.launch │ ├── package.xml │ ├── src │ │ ├── drive_bot.cpp │ │ └── process_image.cpp │ └── srv │ │ └── DriveToTarget.srv │ └── my_robot │ ├── CMakeLists.txt │ ├── config │ ├── base_local_planner_params.yaml │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ └── local_costmap_params.yaml │ ├── launch │ ├── amcl.launch │ ├── robot_description.launch │ └── world.launch │ ├── maps │ ├── map.pgm │ └── map.yaml │ ├── meshes │ └── hokuyo.dae │ ├── package.xml │ ├── urdf │ ├── my_robot.gazebo │ └── my_robot.xacro │ └── world │ └── andrews.world ├── P4_Mapping ├── .gitignore ├── README.md └── src │ ├── CMakeLists.txt │ └── my_robot │ ├── CMakeLists.txt │ ├── config │ ├── base_local_planner_params.yaml │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ └── local_costmap_params.yaml │ ├── default.rviz │ ├── launch │ ├── localization.launch │ ├── mapping.launch │ ├── robot_description.launch │ └── world.launch │ ├── meshes │ └── hokuyo.dae │ ├── package.xml │ ├── urdf │ ├── my_robot.gazebo │ └── my_robot.xacro │ └── worlds │ └── andrews.world ├── P5_Planning ├── .catkin_workspace ├── .gitignore ├── .student_bashrc ├── README.md ├── model │ ├── corridor │ │ ├── model.config │ │ └── model.sdf │ ├── corridor2 │ │ ├── corridor2 │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── model.config │ │ └── model.sdf │ └── simple │ │ ├── model.config │ │ └── model.sdf ├── rvizConfig ├── scripts │ ├── add_markers.sh │ ├── home_service.sh │ ├── pick_objects.sh │ ├── test_navigation.sh │ └── test_slam.sh └── src │ ├── CMakeLists.txt │ ├── add_markers │ ├── CMakeLists.txt │ ├── config │ │ ├── pickplace.yaml │ │ ├── simple.pgm │ │ ├── simple.world │ │ └── simple.yaml │ ├── launch │ │ └── add_markers_rviz.launch │ ├── package.xml │ ├── rvizConfig │ │ ├── default.rviz │ │ └── launch │ │ │ └── rvizConfig.launch │ └── src │ │ ├── add_markers_demo.cpp │ │ └── add_markers_node.cpp │ ├── add_markers_node.cpp │ ├── pick_objects │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ │ └── pick_objects_node.cpp │ └── pick_objects_node.cpp └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "P3_Localization/P3_Localization/src/pgm_map_creator"] 2 | path = P3_Localization/P3_Localization/src/pgm_map_creator 3 | url = https://github.com/udacity/pgm_map_creator 4 | [submodule "P4_Mapping/src/teleop_twist_keyboard"] 5 | path = P4_Mapping/src/teleop_twist_keyboard 6 | url = https://github.com/ros-teleop/teleop_twist_keyboard 7 | [submodule "P5_Planning/src/slam_gmapping"] 8 | path = P5_Planning/src/slam_gmapping 9 | url = https://github.com/ros-perception/slam_gmapping 10 | [submodule "P3_Localization/src/pgm_map_creator"] 11 | path = P3_Localization/src/pgm_map_creator 12 | url = https://github.com/udacity/pgm_map_creator 13 | [submodule "P5_Planning/src/turtlebot_simulator"] 14 | path = P5_Planning/src/turtlebot_simulator 15 | url = https://github.com/turtlebot/turtlebot_simulator 16 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Andrew Presland 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /P1_Simulation/.gitignore: -------------------------------------------------------------------------------- 1 | build/ -------------------------------------------------------------------------------- /P1_Simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | 3 | find_package(gazebo REQUIRED) 4 | include_directories(${GAZEBO_INCLUDE_DIRS}) 5 | link_directories(${GAZEBO_LIBRARY_DIRS}) 6 | list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") 7 | 8 | add_library(hello SHARED script/hello.cpp) 9 | target_link_libraries(hello ${GAZEBO_LIBRARIES}) -------------------------------------------------------------------------------- /P1_Simulation/README.md: -------------------------------------------------------------------------------- 1 | # Robotic Environment Simulation 2 | ![gazebo_simulation](https://user-images.githubusercontent.com/5468707/121535884-d8bde100-ca02-11eb-96a3-1981ffe1a6a7.png) 3 | 4 | ## Prerequisites 5 | This project requires that ROS and Gazebo are installed along with the gcc compiler. 6 | 7 | ## Build 8 | * Change to the project folder 9 | * Create a build folder `mkdir build && cd build` 10 | * Build with cmake `cmake .. && make` 11 | * Add the build folder to the Gazebo plugin path: `GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:`. 12 | * Change to the project folder 13 | * Launch the simulation `gazebo world/office` 14 | 15 | ## Structure 16 | ``` 17 | . 18 | ├── CMakeLists.txt 19 | ├── model 20 | │   ├── Building 21 | │   │   ├── model.config 22 | │   │   └── model.sdf 23 | │   └── Robot 24 | │   ├── model.config 25 | │   └── model.sdf 26 | ├── README.md 27 | ├── script 28 | │   └── hello.cpp 29 | └── world 30 | └── office 31 | ``` 32 | -------------------------------------------------------------------------------- /P1_Simulation/model/Building/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Building 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /P1_Simulation/model/Robot/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Robot 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /P1_Simulation/script/hello.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace gazebo 4 | { 5 | class WorldPluginMyRobot : public WorldPlugin 6 | { 7 | public: WorldPluginMyRobot() : WorldPlugin() 8 | { 9 | printf("Welcome to Andrew’s World!\n"); 10 | } 11 | 12 | public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf) 13 | { 14 | } 15 | }; 16 | GZ_REGISTER_WORLD_PLUGIN(WorldPluginMyRobot) 17 | } -------------------------------------------------------------------------------- /P2_Perception/.gitignore: -------------------------------------------------------------------------------- 1 | .catkin_workspace 2 | build/ 3 | devel/ -------------------------------------------------------------------------------- /P2_Perception/README.md: -------------------------------------------------------------------------------- 1 | # Perception 2 | 3 | alt text 4 | 5 | ## Prerequisites 6 | This project requires ROS and Gazebo along with the gcc compiler 7 | 8 | ## Build 9 | 1. Initialize the project as a catkin workspace 10 | ```console 11 | $ cd src && catkin_init_workspace 12 | ``` 13 | 14 | 2. Move to the project director and build 15 | ```console 16 | $ cd .. 17 | $ catkin_make 18 | ``` 19 | 20 | 3. Launch the simulation 21 | ```console 22 | $ source devel/setup.bash 23 | $ roslaunch my_robot world.launch 24 | ``` 25 | 4. Start the ball chaser in another terminal 26 | ```console 27 | $ source devel/setup.bash 28 | $ roslaunch ball_chaser ball_chaser.launch 29 | ``` 30 | 31 | ## Structure 32 | ``` 33 | . 34 | └── src 35 | ├── ball_chaser 36 | │   ├── CMakeLists.txt 37 | │   ├── include 38 | │   │   └── ball_chaser 39 | │   ├── launch 40 | │   │   └── ball_chaser.launch 41 | │   ├── package.xml 42 | │   ├── src 43 | │   │   ├── drive_bot.cpp 44 | │   │   └── process_image.cpp 45 | │   └── srv 46 | │   └── DriveToTarget.srv 47 | ├── CMakeLists.txt 48 | └── my_robot 49 | ├── CMakeLists.txt 50 | ├── launch 51 | │   ├── robot_description.launch 52 | │   └── world.launch 53 | ├── meshes 54 | │   └── hokuyo.dae 55 | ├── package.xml 56 | ├── urdf 57 | │   ├── my_robot.gazebo 58 | │   └── my_robot.xacro 59 | └── world 60 | └── andrews.world 61 | 62 | ``` 63 | -------------------------------------------------------------------------------- /P2_Perception/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /P2_Perception/src/ball_chaser/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ball_chaser) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | message_generation 12 | roscpp 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | add_service_files( 58 | FILES 59 | DriveToTarget.srv 60 | ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | generate_messages( 71 | DEPENDENCIES 72 | std_msgs 73 | ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES ball_chaser 107 | # CATKIN_DEPENDS message_generation roscpp std_msgs 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/ball_chaser.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/ball_chaser_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # catkin_install_python(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables for installation 167 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 168 | # install(TARGETS ${PROJECT_NAME}_node 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark libraries for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 174 | # install(TARGETS ${PROJECT_NAME} 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ball_chaser.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | 207 | ################# 208 | ## Application ## 209 | ################# 210 | 211 | include_directories(include ${catkin_INCLUDE_DIRS}) 212 | 213 | add_executable(drive_bot src/drive_bot.cpp) 214 | target_link_libraries(drive_bot ${catkin_LIBRARIES}) 215 | add_dependencies(drive_bot ball_chaser_generate_messages_cpp) 216 | 217 | add_executable(process_image src/process_image.cpp) 218 | target_link_libraries(process_image ${catkin_LIBRARIES}) 219 | add_dependencies(process_image ball_chaser_generate_messages_cpp) 220 | -------------------------------------------------------------------------------- /P2_Perception/src/ball_chaser/launch/ball_chaser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /P2_Perception/src/ball_chaser/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ball_chaser 4 | 0.0.0 5 | The ball_chaser package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | message_generation 53 | roscpp 54 | std_msgs 55 | roscpp 56 | std_msgs 57 | roscpp 58 | std_msgs 59 | 60 | sensor_msgs 61 | sensor_msgs 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /P2_Perception/src/ball_chaser/src/drive_bot.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "ros/ros.h" 3 | #include "geometry_msgs/Twist.h" 4 | #include "ball_chaser/DriveToTarget.h" 5 | 6 | // ROS::Publisher motor commands; 7 | ros::Publisher motor_command_publisher; 8 | 9 | // A handle_drive_request callback function that executes whenever a drive_bot service is requested 10 | bool handle_drive_request( 11 | ball_chaser::DriveToTarget::Request& req, 12 | ball_chaser::DriveToTarget::Response& res) 13 | { 14 | ROS_INFO("DriveToTargetRequest received - linear.x:%1.2f, angular.z:%1.2f", 15 | (float)req.linear_x, 16 | (float)req.angular_z); 17 | 18 | 19 | 20 | // Requested velocities 21 | std::vector velocity = { (float)req.linear_x, (float)req.angular_z }; 22 | 23 | // Publish angles to drive the robot 24 | geometry_msgs::Twist motor_command; 25 | motor_command.linear.x = velocity[0]; 26 | motor_command.angular.z = velocity[1]; 27 | motor_command_publisher.publish(motor_command); 28 | 29 | // Return a response message 30 | res.msg_feedback = "Wheel velocity set - linear.x: " 31 | + std::to_string(velocity[0]) 32 | + " , angular.z: " 33 | + std::to_string(velocity[1]); 34 | ROS_INFO_STREAM(res.msg_feedback); 35 | 36 | return true; 37 | } 38 | 39 | int main(int argc, char** argv) 40 | { 41 | // Initialize a ROS node 42 | ros::init(argc, argv, "drive_bot"); 43 | 44 | // Create a ROS NodeHandle object 45 | ros::NodeHandle n; 46 | 47 | // Inform ROS master that we will be publishing a message of type geometry_msgs::Twist 48 | // on the robot actuation topic with a publishing queue size of 10 49 | motor_command_publisher = n.advertise("/cmd_vel", 10); 50 | 51 | ros::ServiceServer service = n.advertiseService( 52 | "/ball_chaser/command_robot", handle_drive_request); 53 | ROS_INFO("Ready to send drive commands"); 54 | 55 | ros::spin(); 56 | 57 | return 0; 58 | } -------------------------------------------------------------------------------- /P2_Perception/src/ball_chaser/src/process_image.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "ball_chaser/DriveToTarget.h" 3 | #include 4 | 5 | // Define a global client that can request services 6 | ros::ServiceClient client; 7 | 8 | // This function calls the command_robot service to drive the robot in the specified direction 9 | void drive_robot(float lin_x, float ang_z) 10 | { 11 | // TODO: Request a service and pass the velocities to it to drive the robot 12 | ROS_INFO("Driving robot to target - linear.x:%1.2f, angular.z:%1.2f", lin_x, ang_z); 13 | 14 | // Request drive to target 15 | ball_chaser::DriveToTarget srv; 16 | srv.request.linear_x = lin_x; 17 | srv.request.angular_z = ang_z; 18 | 19 | // Call the ball_chaser service and pass the requested target 20 | if (!client.call(srv)) 21 | ROS_ERROR("Failed to call service ball_chaser::DriveToTarget"); 22 | } 23 | 24 | // This callback function continuously executes and reads the image data 25 | void process_image_callback(const sensor_msgs::Image img) 26 | { 27 | // get pixel location of ball 28 | int pixel = -1; 29 | for (int i = 0; i < img.height * img.step; i+=3) { 30 | 31 | bool is_max[3] = {false}; 32 | 33 | is_max[0] = (255 == img.data[i]); 34 | is_max[1] = (255 == img.data[i+1]); 35 | is_max[2] = (255 == img.data[i+2]); 36 | 37 | if (is_max[0] && is_max[1] && is_max[2]) { 38 | pixel = i % img.step; 39 | break; 40 | } 41 | } 42 | 43 | float x,z =0.0; // stationary (default) 44 | 45 | if (pixel >= 0) { 46 | 47 | if (pixel < img.step / 3 ) { 48 | z =0.2; // turn left 49 | } 50 | 51 | else if (pixel > 2 * img.step / 3 ) { 52 | z =-0.2; // turn right 53 | } 54 | 55 | else { 56 | x =0.2; // go straight 57 | } 58 | } 59 | 60 | drive_robot(x,z); 61 | } 62 | 63 | int main(int argc, char** argv) 64 | { 65 | // Initialize the process_image node and create a handle to it 66 | ros::init(argc, argv, "process_image"); 67 | ros::NodeHandle n; 68 | 69 | // Define a client service capable of requesting services from command_robot 70 | client = n.serviceClient("/ball_chaser/command_robot"); 71 | 72 | // Subscribe to /camera/rgb/image_raw topic to read the image data inside the process_image_callback function 73 | ros::Subscriber sub1 = n.subscribe("/camera/rgb/image_raw", 10, process_image_callback); 74 | ROS_INFO("Ready to receive images"); 75 | 76 | // Handle ROS communication events 77 | ros::spin(); 78 | 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /P2_Perception/src/ball_chaser/srv/DriveToTarget.srv: -------------------------------------------------------------------------------- 1 | float64 linear_x 2 | float64 angular_z 3 | --- 4 | string msg_feedback -------------------------------------------------------------------------------- /P2_Perception/src/my_robot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(my_robot) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES my_robot 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/my_robot.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/my_robot_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_my_robot.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /P2_Perception/src/my_robot/launch/robot_description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /P2_Perception/src/my_robot/launch/world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /P2_Perception/src/my_robot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | my_robot 4 | 0.0.0 5 | The my_robot package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /P2_Perception/src/my_robot/urdf/my_robot.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 50.0 7 | / 8 | front_left_wheel_hinge 9 | front_right_wheel_hinge 10 | back_left_wheel_hinge 11 | back_right_wheel_hinge 12 | 0.4 13 | 0.2 14 | base_link 15 | 10 16 | cmd_vel 17 | odom 18 | odom 19 | robot_footprint 20 | cmd_vel 21 | false 22 | true 23 | 0.00001 24 | 0.00001 25 | 0.0001 26 | true 27 | 28 | 52 | 53 | 54 | 55 | 56 | 57 | 30.0 58 | 59 | 1.3962634 60 | 61 | 800 62 | 800 63 | R8G8B8 64 | 65 | 66 | 0.02 67 | 300 68 | 69 | 70 | 71 | true 72 | 0.0 73 | camera 74 | rgb/image_raw 75 | rgb/camera_info 76 | camera 77 | 0.07 78 | 0.0 79 | 0.0 80 | 0.0 81 | 0.0 82 | 0.0 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 0 0 0 0 0 0 91 | false 92 | 40 93 | 94 | 95 | 96 | 720 97 | 1 98 | -1.570796 99 | 1.570796 100 | 101 | 102 | 103 | 0.10 104 | 30.0 105 | 0.01 106 | 107 | 108 | gaussian 109 | 113 | 0.0 114 | 0.01 115 | 116 | 117 | 118 | /scan 119 | hokuyo 120 | 121 | 122 | 123 | 124 | 125 | 126 | -------------------------------------------------------------------------------- /P2_Perception/src/my_robot/urdf/my_robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 0 0 0.1 0 0 0 17 | 18 | 19 | 20 | 21 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 204 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 241 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | -------------------------------------------------------------------------------- /P3_Localization/.gitignore: -------------------------------------------------------------------------------- 1 | .catkin_workspace 2 | build/ 3 | devel/ 4 | src/pgm_map_creator/* -------------------------------------------------------------------------------- /P3_Localization/README.md: -------------------------------------------------------------------------------- 1 | # Localization 2 | 3 | ![localization](https://user-images.githubusercontent.com/5468707/123265743-a2508d80-d4fb-11eb-9c79-727a6fdb5b85.gif) 4 | 5 | 6 | We rely on __AMCL__, a probabilistic localization system for a robot moving in 2D, 7 | which implements the adaptive Monte Carlo localization approach first described by Dieter Fox. 8 | It uses a particle filter to track the pose of a robot against a known map. The map is provided the map_server ROS Node, 9 | which offers map data as a ROS Service. Locomotion is provided by the move_base package providing an implementation of an action 10 | that, given a goal in the world, will attempt to reach it with a mobile base. The move_base node links together a global and local planner 11 | to accomplish its global navigation task. 12 | 13 | ## Prerequisites 14 | 1. ROS and Gazebo 15 | 2. CMake and the GCC compiler 16 | 3. ROS dependencies 17 | ```console 18 | $ sudo apt-get update && sudo apt-get upgrade -y 19 | $ sudo apt-get install ros-${ROS_DISTRO}-map-server 20 | $ sudo apt-get install ros-${ROS_DISTRO}-amcl 21 | $ sudo apt-get install ros-${ROS_DISTRO}-move-base 22 | ``` 23 | 24 | ## Build 25 | 1. Iinitialize catkin workspace 26 | ``` 27 | $ mkdir catkin_ws && cd catkin_ws 28 | $ cd src && catkin_init_workspace 29 | ``` 30 | 31 | 2. Within `src`, clone the `teleop` package 32 | ``` 33 | $ cd src 34 | $ git clone https://github.com/ros-teleop/teleop_twist_keyboard 35 | ``` 36 | 37 | 3. Move back to the project directory and build 38 | ``` 39 | $ cd .. 40 | $ catkin_make 41 | ``` 42 | 43 | 4. Launch the world and robot 44 | ``` 45 | $ source devel/setup.bash 46 | $ roslaunch my_robot world.launch 47 | ``` 48 | 49 | 5. Open another terminal and launch the map_server, amcl, and move_back packages will be launched. 50 | ``` 51 | $ source devel/setup.bash 52 | $ roslaunch my_robot amcl.launch 53 | ``` 54 | 55 | 6. Open another terminal, and run the `teleop` node and move around. 56 | ``` 57 | $ source devel/setup.bash 58 | $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py 59 | ``` 60 | 61 | ## Structure 62 | ``` 63 | . 64 | └── src 65 | ├── ball_chaser 66 | │   ├── CMakeLists.txt 67 | │   ├── include 68 | │   │   └── ball_chaser 69 | │   ├── launch 70 | │   │   └── ball_chaser.launch 71 | │   ├── package.xml 72 | │   ├── src 73 | │   │   ├── drive_bot.cpp 74 | │   │   └── process_image.cpp 75 | │   └── srv 76 | │   └── DriveToTarget.srv 77 | ├── CMakeLists.txt 78 | ├── my_robot 79 | │   ├── CMakeLists.txt 80 | │   ├── config 81 | │   │   ├── base_local_planner_params.yaml 82 | │   │   ├── costmap_common_params.yaml 83 | │   │   ├── global_costmap_params.yaml 84 | │   │   └── local_costmap_params.yaml 85 | │   ├── launch 86 | │   │   ├── amcl.launch 87 | │   │   ├── robot_description.launch 88 | │   │   └── world.launch 89 | │   ├── maps 90 | │   │   ├── map.pgm 91 | │   │   └── map.yaml 92 | │   ├── meshes 93 | │   │   └── hokuyo.dae 94 | │   ├── package.xml 95 | │   ├── urdf 96 | │   │   ├── my_robot.gazebo 97 | │   │   └── my_robot.xacro 98 | │   └── world 99 | │   └── andrews.world 100 | ├── pgm_map_creator 101 | │   ├── CMakeLists.txt 102 | │   ├── launch 103 | │   │   └── request_publisher.launch 104 | │   ├── LICENSE 105 | │   ├── maps 106 | │   │   └── map.pgm 107 | │   ├── msgs 108 | │   │   ├── CMakeLists.txt 109 | │   │   └── collision_map_request.proto 110 | │   ├── package.xml 111 | │   ├── README.md 112 | │   ├── src 113 | │   │   ├── collision_map_creator.cc 114 | │   │   └── request_publisher.cc 115 | │   └── world 116 | │   ├── andrews.world 117 | │   └── udacity_mtv 118 | └── teleop_twist_keyboard 119 | ├── CHANGELOG.rst 120 | ├── CMakeLists.txt 121 | ├── package.xml 122 | ├── README.md 123 | └── teleop_twist_keyboard.py 124 | 125 | ``` 126 | -------------------------------------------------------------------------------- /P3_Localization/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /P3_Localization/src/ball_chaser/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ball_chaser) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | message_generation 12 | roscpp 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | add_service_files( 58 | FILES 59 | DriveToTarget.srv 60 | ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | generate_messages( 71 | DEPENDENCIES 72 | std_msgs 73 | ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES ball_chaser 107 | # CATKIN_DEPENDS message_generation roscpp std_msgs 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/ball_chaser.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/ball_chaser_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # catkin_install_python(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables for installation 167 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 168 | # install(TARGETS ${PROJECT_NAME}_node 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark libraries for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 174 | # install(TARGETS ${PROJECT_NAME} 175 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark cpp header files for installation 181 | # install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | # FILES_MATCHING PATTERN "*.h" 184 | # PATTERN ".svn" EXCLUDE 185 | # ) 186 | 187 | ## Mark other files for installation (e.g. launch and bag files, etc.) 188 | # install(FILES 189 | # # myfile1 190 | # # myfile2 191 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 192 | # ) 193 | 194 | ############# 195 | ## Testing ## 196 | ############# 197 | 198 | ## Add gtest based cpp test target and link libraries 199 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ball_chaser.cpp) 200 | # if(TARGET ${PROJECT_NAME}-test) 201 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 202 | # endif() 203 | 204 | ## Add folders to be run by python nosetests 205 | # catkin_add_nosetests(test) 206 | 207 | ################# 208 | ## Application ## 209 | ################# 210 | 211 | include_directories(include ${catkin_INCLUDE_DIRS}) 212 | 213 | add_executable(drive_bot src/drive_bot.cpp) 214 | target_link_libraries(drive_bot ${catkin_LIBRARIES}) 215 | add_dependencies(drive_bot ball_chaser_generate_messages_cpp) 216 | 217 | add_executable(process_image src/process_image.cpp) 218 | target_link_libraries(process_image ${catkin_LIBRARIES}) 219 | add_dependencies(process_image ball_chaser_generate_messages_cpp) 220 | -------------------------------------------------------------------------------- /P3_Localization/src/ball_chaser/launch/ball_chaser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /P3_Localization/src/ball_chaser/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ball_chaser 4 | 0.0.0 5 | The ball_chaser package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | message_generation 53 | roscpp 54 | std_msgs 55 | roscpp 56 | std_msgs 57 | roscpp 58 | std_msgs 59 | 60 | sensor_msgs 61 | sensor_msgs 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /P3_Localization/src/ball_chaser/src/drive_bot.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "ros/ros.h" 3 | #include "geometry_msgs/Twist.h" 4 | #include "ball_chaser/DriveToTarget.h" 5 | 6 | // ROS::Publisher motor commands; 7 | ros::Publisher motor_command_publisher; 8 | 9 | // A handle_drive_request callback function that executes whenever a drive_bot service is requested 10 | bool handle_drive_request( 11 | ball_chaser::DriveToTarget::Request& req, 12 | ball_chaser::DriveToTarget::Response& res) 13 | { 14 | ROS_INFO("DriveToTargetRequest received - linear.x:%1.2f, angular.z:%1.2f", 15 | (float)req.linear_x, 16 | (float)req.angular_z); 17 | 18 | 19 | 20 | // Requested velocities 21 | std::vector velocity = { (float)req.linear_x, (float)req.angular_z }; 22 | 23 | // Publish angles to drive the robot 24 | geometry_msgs::Twist motor_command; 25 | motor_command.linear.x = velocity[0]; 26 | motor_command.angular.z = velocity[1]; 27 | motor_command_publisher.publish(motor_command); 28 | 29 | // Return a response message 30 | res.msg_feedback = "Wheel velocity set - linear.x: " 31 | + std::to_string(velocity[0]) 32 | + " , angular.z: " 33 | + std::to_string(velocity[1]); 34 | ROS_INFO_STREAM(res.msg_feedback); 35 | 36 | return true; 37 | } 38 | 39 | int main(int argc, char** argv) 40 | { 41 | // Initialize a ROS node 42 | ros::init(argc, argv, "drive_bot"); 43 | 44 | // Create a ROS NodeHandle object 45 | ros::NodeHandle n; 46 | 47 | // Inform ROS master that we will be publishing a message of type geometry_msgs::Twist 48 | // on the robot actuation topic with a publishing queue size of 10 49 | motor_command_publisher = n.advertise("/cmd_vel", 10); 50 | 51 | ros::ServiceServer service = n.advertiseService( 52 | "/ball_chaser/command_robot", handle_drive_request); 53 | ROS_INFO("Ready to send drive commands"); 54 | 55 | ros::spin(); 56 | 57 | return 0; 58 | } -------------------------------------------------------------------------------- /P3_Localization/src/ball_chaser/src/process_image.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "ball_chaser/DriveToTarget.h" 3 | #include 4 | 5 | // Define a global client that can request services 6 | ros::ServiceClient client; 7 | 8 | // This function calls the command_robot service to drive the robot in the specified direction 9 | void drive_robot(float lin_x, float ang_z) 10 | { 11 | // TODO: Request a service and pass the velocities to it to drive the robot 12 | ROS_INFO("Driving robot to target - linear.x:%1.2f, angular.z:%1.2f", lin_x, ang_z); 13 | 14 | // Request drive to target 15 | ball_chaser::DriveToTarget srv; 16 | srv.request.linear_x = lin_x; 17 | srv.request.angular_z = ang_z; 18 | 19 | // Call the ball_chaser service and pass the requested target 20 | if (!client.call(srv)) 21 | ROS_ERROR("Failed to call service ball_chaser::DriveToTarget"); 22 | } 23 | 24 | // This callback function continuously executes and reads the image data 25 | void process_image_callback(const sensor_msgs::Image img) 26 | { 27 | // get pixel location of ball 28 | int pixel = -1; 29 | for (int i = 0; i < img.height * img.step; i+=3) { 30 | 31 | bool is_max[3] = {false}; 32 | 33 | is_max[0] = (255 == img.data[i]); 34 | is_max[1] = (255 == img.data[i+1]); 35 | is_max[2] = (255 == img.data[i+2]); 36 | 37 | if (is_max[0] && is_max[1] && is_max[2]) { 38 | pixel = i % img.step; 39 | break; 40 | } 41 | } 42 | 43 | float x,z =0.0; // stationary (default) 44 | 45 | if (pixel >= 0) { 46 | 47 | if (pixel < img.step / 3 ) { 48 | z =0.2; // turn left 49 | } 50 | 51 | else if (pixel > 2 * img.step / 3 ) { 52 | z =-0.2; // turn right 53 | } 54 | 55 | else { 56 | x =0.2; // go straight 57 | } 58 | } 59 | 60 | drive_robot(x,z); 61 | } 62 | 63 | int main(int argc, char** argv) 64 | { 65 | // Initialize the process_image node and create a handle to it 66 | ros::init(argc, argv, "process_image"); 67 | ros::NodeHandle n; 68 | 69 | // Define a client service capable of requesting services from command_robot 70 | client = n.serviceClient("/ball_chaser/command_robot"); 71 | 72 | // Subscribe to /camera/rgb/image_raw topic to read the image data inside the process_image_callback function 73 | ros::Subscriber sub1 = n.subscribe("/camera/rgb/image_raw", 10, process_image_callback); 74 | ROS_INFO("Ready to receive images"); 75 | 76 | // Handle ROS communication events 77 | ros::spin(); 78 | 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /P3_Localization/src/ball_chaser/srv/DriveToTarget.srv: -------------------------------------------------------------------------------- 1 | float64 linear_x 2 | float64 angular_z 3 | --- 4 | string msg_feedback -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(my_robot) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES my_robot 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/my_robot.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/my_robot_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_my_robot.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | controller_frequency: 10 2 | 3 | TrajectoryPlannerROS: 4 | max_vel_x: 1.0 #0.5 5 | min_vel_x: -0.1 #0.01 6 | max_vel_theta: 1.57 #1.5 7 | 8 | min_in_place_vel_theta: 0.314 9 | 10 | acc_lim_theta: 3.14 11 | acc_lim_x: 2.0 12 | acc_lim_y: 2.0 13 | 14 | sim_time: 1.0 15 | 16 | vx_samples: 5.0 17 | vtheta_samples: 10.0 18 | 19 | pdist_scale: 0.6 #0.6 20 | gdist_scale: 0.8 #0.8 21 | occdist_scale: 0.02 22 | 23 | holonomic_robot: false 24 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | map_type: costmap 2 | 3 | obstacle_range: 1.0 # 2.0 4 | raytrace_range: 2.0 # 3.0 5 | 6 | transform_tolerance: 0.2 # 0.0 7 | 8 | robot_radius: 0.3 # 0.0 9 | inflation_radius: 0.5 # 0.0 10 | cost_scaling_factor: 5.0 11 | 12 | observation_sources: laser_scan_sensor 13 | 14 | laser_scan_sensor: {sensor_frame: hokuyo, data_type: LaserScan, topic: /udacity_bot/laser/scan, marking: true, clearing: true} 15 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: robot_footprint 4 | update_frequency: 20.0 5 | publish_frequency: 5.0 6 | width: 20.0 7 | height: 20.0 8 | resolution: 0.05 9 | static_map: true 10 | rolling_window: false 11 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: robot_footprint 4 | update_frequency: 20.0 5 | publish_frequency: 5.0 6 | width: 10.0 7 | height: 10.0 8 | resolution: 0.05 9 | static_map: false 10 | rolling_window: true 11 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/launch/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/launch/robot_description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/launch/world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apresland/autonomous-mobile-robots/8d970fb57cafb4a5b801f42b07adbd57b4d9a2a1/P3_Localization/src/my_robot/maps/map.pgm -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.01 3 | origin: [-15, -15, 0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | my_robot 4 | 0.0.0 5 | The my_robot package 6 | 7 | 8 | 9 | 10 | root 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/urdf/my_robot.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 10.0 7 | / 8 | front_left_wheel_hinge 9 | front_right_wheel_hinge 10 | back_left_wheel_hinge 11 | back_right_wheel_hinge 12 | 0.4 13 | 0.2 14 | 10 15 | cmd_vel 16 | odom 17 | odom 18 | robot_footprint 19 | cmd_vel 20 | true 21 | world 22 | 1 23 | true 24 | 25 | 49 | 50 | 51 | 52 | 53 | 54 | 30.0 55 | 56 | 1.3962634 57 | 58 | 800 59 | 800 60 | R8G8B8 61 | 62 | 63 | 0.02 64 | 300 65 | 66 | 67 | 68 | true 69 | 0.0 70 | camera 71 | rgb/image_raw 72 | rgb/camera_info 73 | camera 74 | 0.07 75 | 0.0 76 | 0.0 77 | 0.0 78 | 0.0 79 | 0.0 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 0 0 0 0 0 0 88 | false 89 | 40 90 | 91 | 92 | 93 | 720 94 | 1 95 | -1.570796 96 | 1.570796 97 | 98 | 99 | 100 | 0.10 101 | 30.0 102 | 0.01 103 | 104 | 105 | gaussian 106 | 110 | 0.0 111 | 0.01 112 | 113 | 114 | 115 | /scan 116 | hokuyo 117 | 118 | 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /P3_Localization/src/my_robot/urdf/my_robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 0 0 0.1 0 0 0 17 | 18 | 19 | 20 | 21 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 204 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 241 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | -------------------------------------------------------------------------------- /P4_Mapping/.gitignore: -------------------------------------------------------------------------------- 1 | .catkin_workspace 2 | build/ 3 | devel/ 4 | src/pgm_map_creator/* -------------------------------------------------------------------------------- /P4_Mapping/README.md: -------------------------------------------------------------------------------- 1 | # Mapping 2 | ![features](https://user-images.githubusercontent.com/5468707/123142323-98764e00-d459-11eb-8131-f6bb09324e76.png) 3 | 4 | When a robot finds itself in a new environment it must create a map and localise itself within it. Mapping algorithms that can be used include Occupancy Grid Mapping, Grid-based FastSLAM, Graph-SLAM and RTAB-Map. 5 | 6 | [RTAB-Map (Real-Time Appearance-Based Mapping)](http://introlab.github.io/rtabmap/) is a Graph-SLAM approach that performs [Loop Closure](http://www.cds.caltech.edu/~murray/courses/me132-wi11/me132a_lec16.pdf) with [Visual Bag-of-Words](https://www.youtube.com/watch?v=a4cFONdc6nc). Loop closure occurs inside working memory based on features detected with [SURF (Speeded Up Robust Features)](https://people.ee.ethz.ch/~surf/eccv06.pdf) esimating how likely a new image comes from a previous location or a new location. When a loop closure hypothesis is accepted, a new constraint is added to the map’s graph and an optimizer minimizes the mapping errors. A memory management is used to limit the number of locations used for loop closure detection and graph optimization, so that real-time constraints on large-scale environnements are respected. 7 | 8 | In this module [rtabmap-ros](http://wiki.ros.org/rtabmap_ros) (a ROS wrapper around the RTAB-Map) will be used with a RGB-D camera which can generate 3D point clouds of the environment and/or create a 2D occupancy grid map for navigation. We will be generating a 2D occupancy grid map. 9 | 10 | ## Prerequisites 11 | 1. ROS and Gazebo 12 | 2. CMake and the gcc compiler 13 | 3. The rtabmap-ros package 14 | 15 | Install the `rtabmap-ros` package 16 | ```console 17 | $ sudo apt-get install ros-${ROS_DISTRO}-rtabmap-ros 18 | ``` 19 | 20 | ## Build 21 | 1. Clone project and initialize a catkin workspace 22 | ``` 23 | $ cd src && catkin_init_workspace 24 | ``` 25 | 26 | 2. Within the `src` folder, clone the `teleop` project 27 | ``` 28 | $ git clone https://github.com/ros-teleop/teleop_twist_keyboard 29 | ``` 30 | 31 | 3. Move back to the project folder and build 32 | ``` 33 | $ cd .. 34 | $ catkin_make 35 | ``` 36 | 37 | 4. In a terminal Launch the world and robot 38 | ``` 39 | $ source devel/setup.bash 40 | $ roslaunch my_robot world.launch 41 | ``` 42 | 43 | 5. In a second terminal launch the `mapping.launch` file to start the rtabmap-ros node. 44 | ``` 45 | $ source devel/setup.bash 46 | $ roslaunch my_robot mapping.launch 47 | ``` 48 | 49 | 6. In a third terminal run the `teleop` node. 50 | ``` 51 | $ source devel/setup.bash 52 | $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py 53 | ``` 54 | 55 | 7. Use the keyboard to navigate the robot with teleop. The rtabmap-ros will save 56 | the map and trajectory in a database file 57 | `~/.ros/rtabmap.db`. 58 | 59 | 8. In a forth terminal open the database file using `rtabmap-databaseViewer` 60 | ``` 61 | $ rtabmap-databaseViewer ~/.ros/rtabmap.db 62 | ``` 63 | 64 | ## Structure 65 | ``` 66 | . 67 | ├── README.md 68 | └── src 69 | ├── CMakeLists.txt -> /opt/ros/noetic/share/catkin/cmake/toplevel.cmake 70 | ├── my_robot 71 | │   ├── CMakeLists.txt 72 | │   ├── config 73 | │   │   ├── base_local_planner_params.yaml 74 | │   │   ├── costmap_common_params.yaml 75 | │   │   ├── global_costmap_params.yaml 76 | │   │   └── local_costmap_params.yaml 77 | │   ├── default.rviz 78 | │   ├── launch 79 | │   │   ├── localization.launch 80 | │   │   ├── mapping.launch 81 | │   │   ├── robot_description.launch 82 | │   │   └── world.launch 83 | │   ├── meshes 84 | │   │   └── hokuyo.dae 85 | │   ├── package.xml 86 | │   ├── urdf 87 | │   │   ├── my_robot.gazebo 88 | │   │   └── my_robot.xacro 89 | │   └── worlds 90 | │   └── andrews.world 91 | └── teleop_twist_keyboard 92 | ├── CHANGELOG.rst 93 | ├── CMakeLists.txt 94 | ├── package.xml 95 | ├── README.md 96 | └── teleop_twist_keyboard.py 97 | 98 | ``` 99 | -------------------------------------------------------------------------------- /P4_Mapping/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(my_robot) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES my_robot 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/my_robot.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/my_robot_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_my_robot.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | controller_frequency: 10 2 | 3 | TrajectoryPlannerROS: 4 | max_vel_x: 1.0 #0.5 5 | min_vel_x: -0.1 #0.01 6 | max_vel_theta: 1.57 #1.5 7 | 8 | min_in_place_vel_theta: 0.314 9 | 10 | acc_lim_theta: 3.14 11 | acc_lim_x: 2.0 12 | acc_lim_y: 2.0 13 | 14 | sim_time: 1.0 15 | 16 | vx_samples: 5.0 17 | vtheta_samples: 10.0 18 | 19 | pdist_scale: 0.6 #0.6 20 | gdist_scale: 0.8 #0.8 21 | occdist_scale: 0.02 22 | 23 | holonomic_robot: false -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | map_type: costmap 2 | 3 | obstacle_range: 1.0 # 2.0 4 | raytrace_range: 2.0 # 3.0 5 | 6 | transform_tolerance: 0.3 # 0.0 7 | 8 | robot_radius: 0.3 # 0.0 9 | inflation_radius: 0.5 # 0.0 10 | cost_scaling_factor: 5.0 11 | 12 | observation_sources: laser_scan_sensor 13 | 14 | laser_scan_sensor: {sensor_frame: hokuyo, data_type: LaserScan, topic: /udacity_bot/laser/scan, marking: true, clearing: true} -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: robot_footprint 4 | update_frequency: 1.5 5 | publish_frequency: 1.5 6 | width: 20.0 7 | height: 20.0 8 | resolution: 0.05 9 | static_map: true 10 | rolling_window: false -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: robot_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 6 | width: 10.0 7 | height: 10.0 8 | resolution: 0.05 9 | static_map: false 10 | rolling_window: true -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/default.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Camera1 10 | - /Map1 11 | Splitter Ratio: 0.5 12 | Tree Height: 150 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Camera 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: false 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: false 57 | - Alpha: 1 58 | Class: rviz/RobotModel 59 | Collision Enabled: false 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: Links in Alphabetic Order 67 | camera: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | camera_link_optical: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | chassis: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | hokuyo: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | robot_footprint: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | wheel_back_left: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | wheel_back_right: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | wheel_front_left: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | wheel_front_right: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | Name: RobotModel 111 | Robot Description: robot_description 112 | TF Prefix: "" 113 | Update Interval: 0 114 | Value: true 115 | Visual Enabled: true 116 | - Class: rviz/Camera 117 | Enabled: true 118 | Image Rendering: background and overlay 119 | Image Topic: /camera/rgb/image_raw 120 | Name: Camera 121 | Overlay Alpha: 0.5 122 | Queue Size: 2 123 | Transport Hint: raw 124 | Unreliable: false 125 | Value: true 126 | Visibility: 127 | Grid: true 128 | Map: true 129 | MapCloud: true 130 | Odometry: true 131 | RobotModel: true 132 | Value: true 133 | Zoom Factor: 1 134 | - Alpha: 1 135 | Autocompute Intensity Bounds: true 136 | Autocompute Value Bounds: 137 | Max Value: 10 138 | Min Value: -10 139 | Value: true 140 | Axis: Z 141 | Channel Name: intensity 142 | Class: rtabmap_ros/MapCloud 143 | Cloud decimation: 4 144 | Cloud from scan: false 145 | Cloud max depth (m): 4 146 | Cloud min depth (m): 0 147 | Cloud voxel size (m): 0.009999999776482582 148 | Color: 255; 255; 255 149 | Color Transformer: "" 150 | Download graph: false 151 | Download map: false 152 | Enabled: false 153 | Filter ceiling (m): 0 154 | Filter floor (m): 0 155 | Invert Rainbow: false 156 | Max Color: 255; 255; 255 157 | Min Color: 0; 0; 0 158 | Name: MapCloud 159 | Node filtering angle (degrees): 30 160 | Node filtering radius (m): 0 161 | Position Transformer: "" 162 | Queue Size: 10 163 | Size (Pixels): 3 164 | Size (m): 0.009999999776482582 165 | Style: Flat Squares 166 | Topic: /rtabmap/mapData 167 | Unreliable: false 168 | Use Fixed Frame: true 169 | Use rainbow: true 170 | Value: false 171 | - Alpha: 0.699999988079071 172 | Class: rviz/Map 173 | Color Scheme: map 174 | Draw Behind: false 175 | Enabled: true 176 | Name: Map 177 | Topic: /rtabmap/grid_prob_map 178 | Unreliable: false 179 | Use Timestamp: false 180 | Value: true 181 | - Angle Tolerance: 0.10000000149011612 182 | Class: rviz/Odometry 183 | Covariance: 184 | Orientation: 185 | Alpha: 0.5 186 | Color: 255; 255; 127 187 | Color Style: Unique 188 | Frame: Local 189 | Offset: 1 190 | Scale: 1 191 | Value: true 192 | Position: 193 | Alpha: 0.30000001192092896 194 | Color: 204; 51; 204 195 | Scale: 1 196 | Value: true 197 | Value: true 198 | Enabled: true 199 | Keep: 100 200 | Name: Odometry 201 | Position Tolerance: 0.10000000149011612 202 | Queue Size: 10 203 | Shape: 204 | Alpha: 1 205 | Axes Length: 1 206 | Axes Radius: 0.10000000149011612 207 | Color: 255; 25; 0 208 | Head Length: 0.30000001192092896 209 | Head Radius: 0.10000000149011612 210 | Shaft Length: 1 211 | Shaft Radius: 0.05000000074505806 212 | Value: Arrow 213 | Topic: "" 214 | Unreliable: false 215 | Value: true 216 | Enabled: true 217 | Global Options: 218 | Background Color: 48; 48; 48 219 | Default Light: true 220 | Fixed Frame: odom 221 | Frame Rate: 30 222 | Name: root 223 | Tools: 224 | - Class: rviz/Interact 225 | Hide Inactive Objects: true 226 | - Class: rviz/MoveCamera 227 | - Class: rviz/Select 228 | - Class: rviz/FocusCamera 229 | - Class: rviz/Measure 230 | - Class: rviz/SetInitialPose 231 | Theta std deviation: 0.2617993950843811 232 | Topic: /initialpose 233 | X std deviation: 0.5 234 | Y std deviation: 0.5 235 | - Class: rviz/SetGoal 236 | Topic: /move_base_simple/goal 237 | - Class: rviz/PublishPoint 238 | Single click: true 239 | Topic: /clicked_point 240 | Value: true 241 | Views: 242 | Current: 243 | Class: rviz/Orbit 244 | Distance: 18.897754669189453 245 | Enable Stereo Rendering: 246 | Stereo Eye Separation: 0.05999999865889549 247 | Stereo Focal Distance: 1 248 | Swap Stereo Eyes: false 249 | Value: false 250 | Field of View: 0.7853981852531433 251 | Focal Point: 252 | X: 0 253 | Y: 0 254 | Z: 0 255 | Focal Shape Fixed Size: true 256 | Focal Shape Size: 0.05000000074505806 257 | Invert Z Axis: false 258 | Name: Current View 259 | Near Clip Distance: 0.009999999776482582 260 | Pitch: 0.785398006439209 261 | Target Frame: 262 | Yaw: 0.785398006439209 263 | Saved: ~ 264 | Window Geometry: 265 | Camera: 266 | collapsed: false 267 | Displays: 268 | collapsed: false 269 | Height: 877 270 | Hide Left Dock: false 271 | Hide Right Dock: false 272 | QMainWindow State: 000000ff00000000fd000000040000000000000375000002ccfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000000da000000da00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000011e000001ec0000001800ffffff000000010000010f000002ccfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002cc000000b100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006af0000003efc0100000002fb0000000800540069006d00650100000000000006af000002c400fffffffb0000000800540069006d006501000000000000045000000000000000000000021f000002cc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 273 | Selection: 274 | collapsed: false 275 | Time: 276 | collapsed: false 277 | Tool Properties: 278 | collapsed: false 279 | Views: 280 | collapsed: false 281 | Width: 1711 282 | X: 35 283 | Y: 92 284 | -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/launch/localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/launch/mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/launch/robot_description.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/launch/world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 34 | 35 | 36 | 38 | 39 | -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | my_robot 4 | 0.0.0 5 | Udacity robo-nd "map-my-world" package 6 | 7 | 8 | 9 | 10 | andrew.presland 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/urdf/my_robot.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 10.0 7 | / 8 | wheel_front_left_hinge 9 | wheel_front_right_hinge 10 | wheel_back_left_hinge 11 | wheel_back_right_hinge 12 | 0.4 13 | 0.2 14 | 10 15 | cmd_vel 16 | odom 17 | odom 18 | robot_footprint 19 | cmd_vel 20 | true 21 | world 22 | 1 23 | true 24 | 25 | 26 | 27 | 28 | 29 | 30 | 1 31 | 20.0 32 | true 33 | 34 | 1.047 35 | 36 | 640 37 | 480 38 | R8G8B8 39 | 40 | 41 | 42 | 43 | 44 | 0.1 45 | 20 46 | 47 | 48 | 49 | true 50 | 10.0 51 | camera 52 | camera_link_optical 53 | rgb/image_raw 54 | depth/image_raw 55 | depth/points 56 | rgb/camera_info 57 | depth/camera_info 58 | 0.4 59 | 0.07 60 | 0.0 61 | 0.0 62 | 0.0 63 | 0.0 64 | 0.0 65 | 0.0 66 | 0.0 67 | 0.0 68 | 0.0 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 0 0 0 0 0 0 77 | false 78 | 40 79 | 80 | 81 | 82 | 720 83 | 1 84 | -1.570796 85 | 1.570796 86 | 87 | 88 | 89 | 0.10 90 | 30.0 91 | 0.01 92 | 93 | 94 | gaussian 95 | 99 | 0.0 100 | 0.01 101 | 102 | 103 | 104 | /scan 105 | hokuyo 106 | 107 | 108 | 109 | 110 | 111 | -------------------------------------------------------------------------------- /P4_Mapping/src/my_robot/urdf/my_robot.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 0 0 0.1 0 0 0 19 | 20 | 21 | 22 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | Gazebo/DarkYellow 274 | 275 | 276 | 277 | Gazebo/DarkGrey 278 | 279 | 280 | 281 | Gazebo/DarkGrey 282 | 283 | 284 | 285 | Gazebo/DarkGrey 286 | 287 | 288 | 289 | Gazebo/DarkGrey 290 | 291 | 292 | 293 | Gazebo/Grey 294 | 295 | 296 | -------------------------------------------------------------------------------- /P5_Planning/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /P5_Planning/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | devel/ -------------------------------------------------------------------------------- /P5_Planning/.student_bashrc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apresland/autonomous-mobile-robots/8d970fb57cafb4a5b801f42b07adbd57b4d9a2a1/P5_Planning/.student_bashrc -------------------------------------------------------------------------------- /P5_Planning/model/corridor/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | corridor 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /P5_Planning/model/corridor/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -0.0095 0 0 0 -0 0 5 | 6 | 7 | 8 | 9 | 10.15 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 | 10.15 0.15 2.5 19 | 20 | 21 | 22 | 26 | 0.760784 0.662745 0.627451 1 27 | 28 | 29 | 0 30 | 31 | 32 | 0.0095 -5 0 0 -0 0 33 | 34 | 35 | 36 | 37 | 38 | 10.15 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 | 10.15 0.15 2.5 48 | 49 | 50 | 51 | 55 | 0.760784 0.662745 0.627451 1 56 | 57 | 58 | 0 59 | 60 | 61 | 5.0095 0 0 0 -0 1.5708 62 | 63 | 64 | 65 | 66 | 67 | 10.15 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 | 10.15 0.15 2.5 77 | 78 | 79 | 80 | 84 | 0.760784 0.662745 0.627451 1 85 | 86 | 87 | 0 88 | 89 | 90 | 0.0095 5 0 0 -0 3.14159 91 | 92 | 93 | 94 | 95 | 96 | 10.15 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 | 10.15 0.15 2.5 106 | 107 | 108 | 109 | 113 | 0.760784 0.662745 0.627451 1 114 | 115 | 116 | 0 117 | 118 | 119 | -4.9905 0 0 0 0 -1.5708 120 | 121 | 122 | 123 | 124 | 125 | 6.769 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 | 6.769 0.15 2.5 135 | 136 | 137 | 138 | 142 | 0.760784 0.662745 0.627451 1 143 | 144 | 145 | 0 146 | 147 | 148 | -1.7 -1.6 0 0 -0 0 149 | 150 | 151 | 152 | 153 | 154 | 6.81269 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 | 6.81269 0.15 2.5 164 | 165 | 166 | 167 | 171 | 0.760784 0.662745 0.627451 1 172 | 173 | 174 | 0 175 | 176 | 177 | 1.678 1.655 0 0 -0 3.14159 178 | 179 | 1 180 | 181 | 182 | -------------------------------------------------------------------------------- /P5_Planning/model/corridor2/corridor2/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | corridor2 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /P5_Planning/model/corridor2/corridor2/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -1.675 -1.79 0 0 -0 0 5 | 6 | 7 | 8 | 9 | 4 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 | 4 0.15 2.5 19 | 20 | 21 | 22 | 26 | 1 1 1 1 27 | 28 | 29 | 0 30 | 31 | 32 | 0 0 0 0 -0 3.14159 33 | 34 | 1 35 | 36 | 37 | -------------------------------------------------------------------------------- /P5_Planning/model/corridor2/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | corridor2 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /P5_Planning/model/corridor2/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 -1.25 0 0 -0 0 5 | 6 | 7 | 8 | 9 | 7.65 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 | 7.65 0.15 2.5 19 | 20 | 21 | 22 | 26 | 0 0.333333 1 1 27 | 28 | 29 | 0 30 | 31 | 32 | -5 0 0 0 0 -1.5708 33 | 34 | 35 | 36 | 37 | 38 | 10.15 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 | 10.15 0.15 2.5 48 | 49 | 50 | 51 | 55 | 0 0.333333 1 1 56 | 57 | 58 | 0 59 | 60 | 61 | 0 -3.75 0 0 -0 0 62 | 63 | 64 | 65 | 66 | 67 | 7.65 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 | 7.65 0.15 2.5 77 | 78 | 79 | 80 | 84 | 0 0.333333 1 1 85 | 86 | 87 | 0 88 | 89 | 90 | 5 0 0 0 -0 1.5708 91 | 92 | 93 | 94 | 95 | 96 | 2.65 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 | 2.65 0.15 2.5 106 | 107 | 108 | 109 | 113 | 0 0.333333 1 1 114 | 115 | 116 | 0 117 | 118 | 119 | 3.75 3.75 0 0 -0 3.14159 120 | 121 | 122 | 123 | 124 | 125 | 5.15 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 | 5.15 0.15 2.5 135 | 136 | 137 | 138 | 142 | 0 0.333333 1 1 143 | 144 | 145 | 0 146 | 147 | 148 | 2.5 1.25 0 0 0 -1.5708 149 | 150 | 151 | 152 | 153 | 154 | 5.15 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 | 5.15 0.15 2.5 164 | 165 | 166 | 167 | 171 | 0 0.333333 1 1 172 | 173 | 174 | 0 175 | 176 | 177 | 0 -1.25 0 0 -0 3.14159 178 | 179 | 180 | 181 | 182 | 183 | 5.15 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 | 5.15 0.15 2.5 193 | 194 | 195 | 196 | 200 | 0 0.333333 1 1 201 | 202 | 203 | 0 204 | 205 | 206 | -2.5 1.25 0 0 -0 1.5708 207 | 208 | 209 | 210 | 211 | 212 | 2.65 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 | 2.65 0.15 2.5 222 | 223 | 224 | 225 | 229 | 0 0.333333 1 1 230 | 231 | 232 | 0 233 | 234 | 235 | -3.75 3.75 0 0 -0 3.14159 236 | 237 | 1 238 | 239 | 240 | -------------------------------------------------------------------------------- /P5_Planning/model/simple/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | simple 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /P5_Planning/model/simple/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0.29 0.2 0 0 -0 0 5 | 6 | 7 | 8 | 9 | 5.85 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 | 5.85 0.15 2.5 19 | 20 | 21 | 22 | 26 | 1 1 1 1 27 | 28 | 29 | 0 30 | 31 | 32 | 0 -2.85 0 0 -0 3.14159 33 | 34 | 35 | 36 | 37 | 38 | 5.85 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 | 5.85 0.15 2.5 48 | 49 | 50 | 51 | 55 | 1 1 1 1 56 | 57 | 58 | 0 59 | 60 | 61 | 2.85 0 0 0 0 -1.5708 62 | 63 | 64 | 65 | 66 | 67 | 3 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 | 3 0.15 2.5 77 | 78 | 79 | 80 | 84 | 1 1 1 1 85 | 86 | 87 | 0 88 | 89 | 90 | -2.85 -1.425 0 0 -0 1.5708 91 | 92 | 93 | 94 | 95 | 96 | 3 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 | 3 0.15 2.5 106 | 107 | 108 | 109 | 113 | 1 1 1 1 114 | 115 | 116 | 0 117 | 118 | 119 | -1.425 0 0 0 -0 0 120 | 121 | 122 | 123 | 124 | 125 | 3 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 | 3 0.15 2.5 135 | 136 | 137 | 138 | 142 | 1 1 1 1 143 | 144 | 145 | 0 146 | 147 | 148 | 0 1.425 0 0 -0 1.5708 149 | 150 | 151 | 152 | 153 | 154 | 3 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 | 3 0.15 2.5 164 | 165 | 166 | 167 | 171 | 1 1 1 1 172 | 173 | 174 | 0 175 | 176 | 177 | 1.425 2.85 0 0 -0 0 178 | 179 | 1 180 | 181 | 182 | -------------------------------------------------------------------------------- /P5_Planning/rvizConfig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apresland/autonomous-mobile-robots/8d970fb57cafb4a5b801f42b07adbd57b4d9a2a1/P5_Planning/rvizConfig -------------------------------------------------------------------------------- /P5_Planning/scripts/add_markers.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | xterm -e " 4 | roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=$(rospack find turtlebot_gazebo)/worlds/simple.world " & 5 | sleep 5 6 | 7 | xterm -e " 8 | roslaunch turtlebot_gazebo amcl_demo.launch map_file:=$(rospack find turtlebot_gazebo)/maps/simple.yaml" & 9 | sleep 5 10 | 11 | xterm -e " 12 | roslaunch add_markers add_markers_rviz.launch rviz_config_file:=$(rospack find add_markers)/rvizConfig/default.rviz" & 13 | sleep 5 14 | 15 | xterm -e " 16 | rosparam load $(rospack find add_markers)/config/pickplace.yaml; 17 | rosrun add_markers add_markers_demo " & 18 | -------------------------------------------------------------------------------- /P5_Planning/scripts/home_service.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | xterm -e " 4 | roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=$(rospack find add_markers)/config/simple.world " & 5 | sleep 5 6 | 7 | xterm -e " 8 | roslaunch turtlebot_gazebo amcl_demo.launch map_file:=$(rospack find add_markers)/config/simple.yaml" & 9 | sleep 5 10 | 11 | xterm -e " 12 | roslaunch add_markers add_markers_rviz.launch rviz_config_file:=$(rospack find add_markers)/rvizConfig/default.rviz" & 13 | sleep 5 14 | 15 | xterm -e " 16 | rosparam load $(rospack find add_markers)/config/pickplace.yaml; 17 | rosrun add_markers add_markers_node " & 18 | sleep 5 19 | 20 | xterm -e " 21 | rosparam load $(rospack find add_markers)/config/pickplace.yaml; 22 | rosrun pick_objects pick_objects_node " & 23 | 24 | 25 | -------------------------------------------------------------------------------- /P5_Planning/scripts/pick_objects.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | xterm -e " 4 | roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=$(rospack find turtlebot_gazebo)/worlds/simple.world " & 5 | sleep 5 6 | 7 | xterm -e " 8 | roslaunch turtlebot_gazebo amcl_demo.launch map_file:=$(rospack find turtlebot_gazebo)/maps/simple.yaml" & 9 | sleep 5 10 | 11 | xterm -e " 12 | roslaunch turtlebot_rviz_launchers view_navigation.launch" & 13 | sleep 5 14 | 15 | xterm -e " 16 | rosparam load $(rospack find add_markers)/config/pickplace.yaml; 17 | rosrun pick_objects pick_objects_node " & 18 | -------------------------------------------------------------------------------- /P5_Planning/scripts/test_navigation.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | xterm -e " 4 | roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=$(rospack find turtlebot_gazebo)/worlds/simple.world" & 5 | sleep 5 6 | 7 | xterm -e " 8 | roslaunch turtlebot_gazebo amcl_demo.launch map_file:=$(rospack find turtlebot_gazebo)/maps/simple.yaml" & 9 | sleep 5 10 | 11 | xterm -e " 12 | roslaunch turtlebot_rviz_launchers view_navigation.launch " & 13 | sleep 5 14 | -------------------------------------------------------------------------------- /P5_Planning/scripts/test_slam.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | xterm -e " 4 | roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=$(rospack find turtlebot_gazebo)/worlds/simple.world" & 5 | sleep 5 6 | 7 | xterm -e " 8 | roslaunch turtlebot_gazebo gmapping_demo.launch " & 9 | sleep 5 10 | 11 | xterm -e " 12 | roslaunch turtlebot_rviz_launchers view_navigation.launch " & 13 | sleep 5 14 | 15 | xterm -e " 16 | roslaunch turtlebot_teleop keyboard_teleop.launch " & 17 | -------------------------------------------------------------------------------- /P5_Planning/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /P5_Planning/src/add_markers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(add_markers) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | visualization_msgs 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # visualization_msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES add_markers 107 | # CATKIN_DEPENDS roscpp visualization_msgs 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | #add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/add_markers.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | add_executable(${PROJECT_NAME}_node src/add_markers_node.cpp) 136 | add_executable(${PROJECT_NAME}_demo src/add_markers_demo.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | target_link_libraries(${PROJECT_NAME}_node 150 | ${catkin_LIBRARIES} 151 | ) 152 | target_link_libraries(${PROJECT_NAME}_demo 153 | ${catkin_LIBRARIES} 154 | ) 155 | 156 | ############# 157 | ## Install ## 158 | ############# 159 | 160 | # all install targets should use catkin DESTINATION variables 161 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 162 | 163 | ## Mark executable scripts (Python etc.) for installation 164 | ## in contrast to setup.py, you can choose the destination 165 | # catkin_install_python(PROGRAMS 166 | # scripts/my_python_script 167 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 168 | # ) 169 | 170 | ## Mark executables for installation 171 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 172 | # install(TARGETS ${PROJECT_NAME}_node 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark libraries for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 178 | # install(TARGETS ${PROJECT_NAME} 179 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark cpp header files for installation 185 | # install(DIRECTORY include/${PROJECT_NAME}/ 186 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 187 | # FILES_MATCHING PATTERN "*.h" 188 | # PATTERN ".svn" EXCLUDE 189 | # ) 190 | 191 | ## Mark other files for installation (e.g. launch and bag files, etc.) 192 | # install(FILES 193 | # # myfile1 194 | # # myfile2 195 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 196 | # ) 197 | 198 | ############# 199 | ## Testing ## 200 | ############# 201 | 202 | ## Add gtest based cpp test target and link libraries 203 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_add_markers.cpp) 204 | # if(TARGET ${PROJECT_NAME}-test) 205 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 206 | # endif() 207 | 208 | ## Add folders to be run by python nosetests 209 | # catkin_add_nosetests(test) 210 | -------------------------------------------------------------------------------- /P5_Planning/src/add_markers/config/pickplace.yaml: -------------------------------------------------------------------------------- 1 | home: 2 | tx: 0.0 3 | ty: 0.0 4 | tz: 0.0 5 | qx: 0.0 6 | qy: 0.0 7 | qz: 0.0 8 | qw: 1.0 9 | 10 | 11 | pick: 12 | tx: 1.5 13 | ty: 0.0 14 | tz: 0.0 15 | qx: 0.0 16 | qy: 0.0 17 | qz: 0.0 18 | qw: 1.0 19 | 20 | place: 21 | tx: 3.5 22 | ty: 3.5 23 | tz: 0.0 24 | qx: 0.0 25 | qy: 0.0 26 | qz: 0.0 27 | qw: 1.0 -------------------------------------------------------------------------------- /P5_Planning/src/add_markers/config/simple.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apresland/autonomous-mobile-robots/8d970fb57cafb4a5b801f42b07adbd57b4d9a2a1/P5_Planning/src/add_markers/config/simple.pgm -------------------------------------------------------------------------------- /P5_Planning/src/add_markers/config/simple.yaml: -------------------------------------------------------------------------------- 1 | image: simple.pgm 2 | resolution: 0.050000 3 | origin: [-1.000000, -12.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /P5_Planning/src/add_markers/launch/add_markers_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /P5_Planning/src/add_markers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | add_markers 4 | 0.0.0 5 | The add_markers package 6 | 7 | 8 | 9 | 10 | robond 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | visualization_msgs 54 | roscpp 55 | visualization_msgs 56 | roscpp 57 | visualization_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /P5_Planning/src/add_markers/rvizConfig/launch/rvizConfig.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 2 | #include 3 | 4 | int main( int argc, char** argv ) { 5 | 6 | ros::init(argc, argv, "add_markers_demo"); 7 | ros::NodeHandle node; 8 | ros::Publisher publisher = node.advertise("visualization_marker", 1); 9 | ros::spinOnce(); 10 | 11 | ROS_INFO("Add Markers demo application"); 12 | 13 | visualization_msgs::Marker marker; 14 | marker.type = visualization_msgs::Marker::CUBE;; 15 | marker.header.frame_id = "odom"; 16 | marker.header.stamp = ros::Time::now(); 17 | marker.ns = "basic_shapes"; 18 | marker.id = 0; 19 | 20 | marker.pose.position.x = 0; 21 | marker.pose.position.y = 0; 22 | marker.pose.position.z = 0; 23 | marker.pose.orientation.x = 0.0; 24 | marker.pose.orientation.y = 0.0; 25 | marker.pose.orientation.z = 0.0; 26 | marker.pose.orientation.w = 1.0; 27 | 28 | marker.scale.x = 0.2; 29 | marker.scale.y = 0.2; 30 | marker.scale.z = 0.2; 31 | 32 | marker.color.r = 1.0f; 33 | marker.color.g = 1.0f; 34 | marker.color.b = 0.0f; 35 | marker.color.a = 1.0; 36 | 37 | marker.lifetime = ros::Duration(); 38 | 39 | sleep(5); 40 | ROS_INFO("Add pick marker"); 41 | node.getParam("/pick/tx", marker.pose.position.x); 42 | node.getParam("/pick/ty", marker.pose.position.y); 43 | node.getParam("/pick/tz", marker.pose.position.z); 44 | node.getParam("/pick/qx", marker.pose.orientation.x); 45 | node.getParam("/pick/qy", marker.pose.orientation.y); 46 | node.getParam("/pick/qz", marker.pose.orientation.z); 47 | node.getParam("/pick/qw", marker.pose.orientation.w); 48 | marker.action = visualization_msgs::Marker::ADD; 49 | publisher.publish(marker); 50 | 51 | sleep(5); 52 | ROS_INFO("Delete pick marker"); 53 | marker.action = visualization_msgs::Marker::DELETE; 54 | publisher.publish(marker); 55 | 56 | sleep(5); 57 | ROS_INFO("Add place marker"); 58 | node.getParam("/place/tx", marker.pose.position.x); 59 | node.getParam("/place/ty", marker.pose.position.y); 60 | node.getParam("/place/tz", marker.pose.position.z); 61 | node.getParam("/place/qx", marker.pose.orientation.x); 62 | node.getParam("/place/qy", marker.pose.orientation.y); 63 | node.getParam("/place/qz", marker.pose.orientation.z); 64 | node.getParam("/place/qw", marker.pose.orientation.w); 65 | marker.action = visualization_msgs::Marker::ADD; 66 | publisher.publish(marker); 67 | 68 | sleep(10); 69 | 70 | return 0; 71 | }; -------------------------------------------------------------------------------- /P5_Planning/src/add_markers/src/add_markers_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | uint8_t state = 99; 6 | 7 | double home[2] = {0.0, 0.0}; 8 | double pick[2] = {1.0, 1.0}; 9 | double place[2] = {3.5, 0.0}; 10 | 11 | void process_state_callback(const std_msgs::UInt8::ConstPtr& msg) 12 | { 13 | state = msg->data; 14 | ROS_INFO("Received process state update %d", state); 15 | return; 16 | } 17 | 18 | int main( int argc, char** argv ) { 19 | 20 | ros::init(argc, argv, "add_markers"); 21 | ros::NodeHandle node; 22 | ros::Publisher publisher = node.advertise("visualization_marker", 1); 23 | ros::Subscriber subscriber = node.subscribe("/process_state", 1, process_state_callback); 24 | 25 | ROS_INFO("Polling for process state updates"); 26 | while (ros::ok()) { 27 | 28 | ros::spinOnce(); 29 | 30 | visualization_msgs::Marker marker; 31 | marker.type = visualization_msgs::Marker::CUBE;; 32 | marker.header.frame_id = "odom"; 33 | marker.header.stamp = ros::Time::now(); 34 | marker.ns = "basic_shapes"; 35 | marker.id = 0; 36 | 37 | marker.pose.position.x = 0; 38 | marker.pose.position.y = 0; 39 | marker.pose.position.z = 0; 40 | marker.pose.orientation.x = 0.0; 41 | marker.pose.orientation.y = 0.0; 42 | marker.pose.orientation.z = 0.0; 43 | marker.pose.orientation.w = 1.0; 44 | 45 | marker.scale.x = 0.2; 46 | marker.scale.y = 0.2; 47 | marker.scale.z = 0.2; 48 | 49 | marker.color.r = 1.0f; 50 | marker.color.g = 1.0f; 51 | marker.color.b = 0.0f; 52 | marker.color.a = 1.0; 53 | 54 | marker.lifetime = ros::Duration(); 55 | 56 | switch (state) { 57 | case 0: { 58 | node.getParam("/pick/tx", marker.pose.position.x); 59 | node.getParam("/pick/ty", marker.pose.position.y); 60 | node.getParam("/pick/tz", marker.pose.position.z); 61 | node.getParam("/pick/qx", marker.pose.orientation.x); 62 | node.getParam("/pick/qy", marker.pose.orientation.y); 63 | node.getParam("/pick/qz", marker.pose.orientation.z); 64 | node.getParam("/pick/qw", marker.pose.orientation.w); 65 | marker.action = visualization_msgs::Marker::ADD; 66 | } break; 67 | case 1: 68 | case 2: { 69 | marker.action = visualization_msgs::Marker::DELETE; 70 | } break; 71 | case 3 : 72 | case 4 : { 73 | node.getParam("/place/tx", marker.pose.position.x); 74 | node.getParam("/place/ty", marker.pose.position.y); 75 | node.getParam("/place/tz", marker.pose.position.z); 76 | node.getParam("/place/qx", marker.pose.orientation.x); 77 | node.getParam("/place/qy", marker.pose.orientation.y); 78 | node.getParam("/place/qz", marker.pose.orientation.z); 79 | node.getParam("/place/qw", marker.pose.orientation.w); 80 | marker.action = visualization_msgs::Marker::ADD; 81 | } break; 82 | }; 83 | 84 | publisher.publish(marker); 85 | 86 | }; 87 | 88 | return 0; 89 | }; -------------------------------------------------------------------------------- /P5_Planning/src/add_markers_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | class AddMarkersNode { 6 | 7 | public: 8 | 9 | enum PickState : int { PICK, MOVE, PLACE, IDLE }; 10 | PickState state = PickState::PICK; 11 | 12 | ros::NodeHandle node; 13 | ros::Publisher marker_publisher; 14 | ros::Subscriber odometry_subscriber; 15 | ros::Subscriber target_subscriber; 16 | 17 | geometry_msgs::Pose odometry; 18 | geometry_msgs::Pose target; 19 | 20 | visualization_msgs::Marker marker; 21 | 22 | double pick[2] = {1.0, 1.0}; 23 | double place[2] = {4.0, 3.0}; 24 | 25 | ros::Time stamp; 26 | 27 | AddMarkersNode(); 28 | 29 | void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg); 30 | void targetCallback(const geometry_msgs::Pose &msg); 31 | void update(); 32 | }; 33 | 34 | AddMarkersNode::AddMarkersNode() { 35 | 36 | marker_publisher = node.advertise("visualization_marker", 1); 37 | odometry_subscriber = node.subscribe("/odom", 1, &AddMarkersNode::odometryCallback, this); 38 | target_subscriber = node.subscribe("/target", 1, &AddMarkersNode::targetCallback, this); 39 | stamp = ros::Time::now(); 40 | 41 | marker.type = visualization_msgs::Marker::CUBE;; 42 | marker.header.frame_id = "odom"; 43 | marker.header.stamp = ros::Time::now(); 44 | marker.ns = "basic_shapes"; 45 | marker.id = 0; 46 | 47 | marker.pose.position.x = 0; 48 | marker.pose.position.y = 0; 49 | marker.pose.position.z = 0; 50 | marker.pose.orientation.x = 0.0; 51 | marker.pose.orientation.y = 0.0; 52 | marker.pose.orientation.z = 0.0; 53 | marker.pose.orientation.w = 1.0; 54 | 55 | marker.scale.x = 0.1; 56 | marker.scale.y = 0.1; 57 | marker.scale.z = 0.1; 58 | 59 | marker.color.r = 1.0f; 60 | marker.color.g = 0.0f; 61 | marker.color.b = 0.0f; 62 | marker.color.a = 1.0; 63 | } 64 | 65 | void AddMarkersNode::odometryCallback(const nav_msgs::Odometry::ConstPtr &msg) { 66 | 67 | ROS_INFO("Odometry update received"); 68 | odometry.position.x = msg->pose.pose.position.x; 69 | odometry.position.y = msg->pose.pose.position.y; 70 | odometry.position.z = msg->pose.pose.position.z; 71 | odometry.orientation.x = msg->pose.pose.orientation.x; 72 | odometry.orientation.y = msg->pose.pose.orientation.y; 73 | odometry.orientation.z = msg->pose.pose.orientation.z; 74 | odometry.orientation.w = msg->pose.pose.orientation.w; 75 | this->update(); 76 | } 77 | 78 | void AddMarkersNode::targetCallback(const geometry_msgs::Pose &msg) { 79 | 80 | ROS_INFO("Target update received"); 81 | target.position.x = msg.position.x; 82 | target.position.y = msg.position.y; 83 | target.position.z = msg.position.z; 84 | target.orientation.x = msg.orientation.x; 85 | target.orientation.y = msg.orientation.y; 86 | target.orientation.z = msg.orientation.z; 87 | target.orientation.w = msg.orientation.w; 88 | this->update(); 89 | } 90 | 91 | void AddMarkersNode::update() 92 | { 93 | switch (state) { 94 | 95 | case PickState::PICK: { 96 | ROS_INFO("Picking from origin"); 97 | marker.pose.position.x = pick[0]; 98 | marker.pose.position.y = pick[1]; 99 | marker.action = visualization_msgs::Marker::ADD; 100 | marker.lifetime = ros::Duration(); 101 | stamp = ros::Time::now(); 102 | state = PickState::MOVE; 103 | } break; 104 | 105 | case PickState::MOVE: { 106 | ROS_INFO("Moving to destination"); 107 | marker.action = visualization_msgs::Marker::DELETE; 108 | marker.lifetime = ros::Duration(); 109 | stamp = ros::Time::now(); 110 | state = PickState::PLACE; 111 | } break; 112 | 113 | case PickState::PLACE: { 114 | ROS_INFO("Placing at destination"); 115 | marker.pose.position.x = place[0]; 116 | marker.pose.position.y = place[1]; 117 | marker.action = visualization_msgs::Marker::ADD; 118 | marker.lifetime = ros::Duration(); 119 | stamp = ros::Time::now(); 120 | state = PickState::IDLE; 121 | } break; 122 | 123 | case PickState::IDLE: { 124 | ROS_INFO("The motion base is idle"); 125 | marker.pose.position.x = place[0]; 126 | marker.pose.position.y = place[1]; 127 | marker.action = visualization_msgs::Marker::ADD; 128 | marker.lifetime = ros::Duration(); 129 | /** nothing to do **/ 130 | } 131 | }; 132 | 133 | marker_publisher.publish(marker); 134 | } 135 | 136 | int main( int argc, char** argv ) 137 | { 138 | ros::init(argc, argv, "add_markers"); 139 | AddMarkersNode node; 140 | ros::Rate r(1); 141 | ros::spin(); 142 | }; -------------------------------------------------------------------------------- /P5_Planning/src/pick_objects/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(pick_objects) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | actionlib 12 | move_base_msgs 13 | roscpp 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # move_base_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES pick_objects 108 | # CATKIN_DEPENDS actionlib move_base_msgs roscpp 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/pick_objects.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | add_executable(${PROJECT_NAME}_node src/pick_objects_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | target_link_libraries(${PROJECT_NAME}_node 150 | ${catkin_LIBRARIES} 151 | ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # catkin_install_python(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_pick_objects.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | -------------------------------------------------------------------------------- /P5_Planning/src/pick_objects/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pick_objects 4 | 0.0.0 5 | The pick_objects package 6 | 7 | 8 | 9 | 10 | robond 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | actionlib 53 | move_base_msgs 54 | roscpp 55 | actionlib 56 | move_base_msgs 57 | roscpp 58 | actionlib 59 | move_base_msgs 60 | roscpp 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /P5_Planning/src/pick_objects/src/pick_objects_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // Define a client for to send goal requests to the move_base server through a SimpleActionClient 7 | typedef actionlib::SimpleActionClient MoveBaseClient; 8 | 9 | double home[2] = {0.0, 0.0}; 10 | double pick[2] = {1.0, 1.0}; 11 | double place[2] = {3.5, 0.0}; 12 | 13 | int main(int argc, char** argv){ 14 | 15 | // Initialize the navigation goal node 16 | ros::init(argc, argv, "pick_objects"); 17 | ros::NodeHandle node; 18 | ros::Publisher publisher = node.advertise("/process_state", 1); 19 | 20 | //tell the action client that we want to spin a thread by default 21 | MoveBaseClient ac("move_base", true); 22 | 23 | // Wait 5 sec for move_base action server to come up 24 | while(!ac.waitForServer(ros::Duration(5.0))){ 25 | ROS_INFO("Waiting for the move_base action server to come up"); 26 | } 27 | 28 | move_base_msgs::MoveBaseGoal goal; 29 | std_msgs::UInt8 state; 30 | 31 | ROS_INFO("Moving to the pick zone"); 32 | state.data = 0; 33 | 34 | goal.target_pose.header.frame_id = "/map"; 35 | goal.target_pose.header.stamp = ros::Time::now(); 36 | node.getParam("/pick/tx", goal.target_pose.pose.position.x); 37 | node.getParam("/pick/ty", goal.target_pose.pose.position.y); 38 | node.getParam("/pick/tz", goal.target_pose.pose.position.z); 39 | node.getParam("/pick/qx", goal.target_pose.pose.orientation.x); 40 | node.getParam("/pick/qy", goal.target_pose.pose.orientation.y); 41 | node.getParam("/pick/qz", goal.target_pose.pose.orientation.z); 42 | node.getParam("/pick/qw", goal.target_pose.pose.orientation.w); 43 | 44 | publisher.publish(state); 45 | ac.sendGoal(goal); 46 | ac.waitForResult(); 47 | 48 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { 49 | ROS_INFO("Attained a pose in the pick zone"); 50 | } else { 51 | ROS_INFO("Did not attain a pose in the pick zone"); 52 | return 0; 53 | } 54 | 55 | ROS_INFO("Picking object"); 56 | sleep(5); 57 | state.data = 1; 58 | publisher.publish(state); 59 | 60 | ROS_INFO("Moving to the place zone"); 61 | state.data = 2; 62 | goal.target_pose.header.frame_id = "/map"; 63 | goal.target_pose.header.stamp = ros::Time::now(); 64 | node.getParam("/place/tx", goal.target_pose.pose.position.x); 65 | node.getParam("/place/ty", goal.target_pose.pose.position.y); 66 | node.getParam("/place/tz", goal.target_pose.pose.position.z); 67 | node.getParam("/place/qx", goal.target_pose.pose.orientation.x); 68 | node.getParam("/place/qy", goal.target_pose.pose.orientation.y); 69 | node.getParam("/place/qz", goal.target_pose.pose.orientation.z); 70 | node.getParam("/place/qw", goal.target_pose.pose.orientation.w); 71 | publisher.publish(state); 72 | ac.sendGoal(goal); 73 | ac.waitForResult(); 74 | 75 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { 76 | ROS_INFO("Attained a pose in the place zone"); 77 | } else { 78 | ROS_INFO("Did not attain a pose in the place zone"); 79 | return 0; 80 | } 81 | 82 | ROS_INFO("Placing object"); 83 | sleep(5); 84 | state.data = 3; 85 | publisher.publish(state); 86 | 87 | 88 | ROS_INFO("Objective complete returning home"); 89 | state.data = 4; 90 | goal.target_pose.header.frame_id = "/map"; 91 | goal.target_pose.header.stamp = ros::Time::now(); 92 | node.getParam("/home/tx", goal.target_pose.pose.position.x); 93 | node.getParam("/home/ty", goal.target_pose.pose.position.y); 94 | node.getParam("/home/tz", goal.target_pose.pose.position.z); 95 | node.getParam("/home/qx", goal.target_pose.pose.orientation.x); 96 | node.getParam("/home/qy", goal.target_pose.pose.orientation.y); 97 | node.getParam("/home/qz", goal.target_pose.pose.orientation.z); 98 | node.getParam("/home/qw", goal.target_pose.pose.orientation.w); 99 | publisher.publish(state); 100 | ac.sendGoal(goal); 101 | ac.waitForResult(); 102 | 103 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { 104 | ROS_INFO("Attained a pose in the home zone"); 105 | } else { 106 | ROS_INFO("Did not attain a pose in the home zone"); 107 | return 0; 108 | } 109 | 110 | sleep(10); 111 | 112 | return 0; 113 | } 114 | -------------------------------------------------------------------------------- /P5_Planning/src/pick_objects_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // Define a client for to send goal requests to the move_base server through a SimpleActionClient 7 | typedef actionlib::SimpleActionClient MoveBaseClient; 8 | 9 | void move_to_goal(MoveBaseClient &ac, const double x, const double y, const double w) { 10 | 11 | move_base_msgs::MoveBaseGoal goal; 12 | 13 | // set up the frame parameters 14 | goal.target_pose.header.frame_id = "odom"; 15 | goal.target_pose.header.stamp = ros::Time::now(); 16 | 17 | // Define a position and orientation for the robot to reach 18 | goal.target_pose.pose.position.x = x; 19 | goal.target_pose.pose.position.y = y; 20 | goal.target_pose.pose.orientation.w = w; 21 | 22 | // Send the goal position and orientation for the robot to reach 23 | ROS_INFO("Sending goal"); 24 | ac.sendGoal(goal); 25 | 26 | // Wait an infinite time for the results 27 | ac.waitForResult(); 28 | 29 | // Check if the robot reached its goal 30 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 31 | ROS_INFO("SUCCEEDED: The base attained goal pose in zone"); 32 | else 33 | ROS_INFO("FAILED: The base failed to attain to goal pose in zone"); 34 | } 35 | 36 | int main(int argc, char** argv){ 37 | 38 | // Initialize the simple_navigation_goals node 39 | ros::init(argc, argv, "pick_objects"); 40 | 41 | //tell the action client that we want to spin a thread by default 42 | MoveBaseClient ac("move_base", true); 43 | 44 | // Wait 5 sec for move_base action server to come up 45 | while(!ac.waitForServer(ros::Duration(5.0))){ 46 | ROS_INFO("Waiting for the move_base action server to come up"); 47 | } 48 | 49 | ros::spinOnce(); 50 | 51 | move_to_goal(ac, 1.0, 1.0, 1.0); 52 | sleep(5); 53 | 54 | move_to_goal(ac, 4.0, 3.0, 1.0); 55 | sleep(5); 56 | 57 | ROS_INFO("All pending actions processed"); 58 | 59 | return 0; 60 | } 61 | --------------------------------------------------------------------------------