diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..6074da1 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,26 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/home/proto/turtleBot3/devel/include/**", + "/opt/ros/noetic/include/**", + "/home/proto/turtleBot3/src/turtlebot3_fake/include/**", + "/home/proto/turtleBot3/src/turtlebot3_gazebo/include/**", + "/usr/include/**", + "${workspaceFolder}/turtlebot3/include", + "${workspaceFolder}/turtlebot3/src" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14", + "configurationProvider": "ms-vscode.cmake-tools" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..834bb17 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,91 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/proto/turtleBot3/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "files.associations": { + "*.ipp": "cpp", + "*.tcc": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "any": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "system_error": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csetjmp": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "bitset": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "cuchar": "cpp", + "map": "cpp", + "set": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "string_view": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "scoped_allocator": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp" + }, + "python.analysis.extraPaths": [ + "/home/proto/turtleBot3/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "C_Cpp.errorSquiggles": "enabled" +} \ No newline at end of file diff --git a/README.md b/README.md index 156cf7b..0b52268 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,35 @@ -# TurtleBot3 - BFS path planning +# TurtleBot3 - Visual Slam and Path Planning with Dynamic Obstacle avoidance ## Objective of the project : -To Understand path planning concepts using slam, and implementing BFS on LiDAR based TurtleBot3, and then further increasing that knowledge and implementing more advanced path planning concepts - -## Reference for the Model:- [Robotis](https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/) +To make Localization through Visual SLAM and making a local costmap using OAKD and global costmap using LIDAR on the TurtleBot3, along with this we need to implement dynamic Obstacle Avoidance + + +## Table of Contents + +- [Project](#objective-of-the-project) + - [Demo](#demo) + - [Table of Contents](#table-of-contents) + - [Stages](#stages) + - [Demo](#demo) + - [Algorithm Flowchart](#algorithm-for-bfs) + - [Our Approach](#pseudocode:using-stacks) + - [Getting Started](#how-to-run-bfs-from-this-project) + +### Stages +* Stage 1: + - [x] Understanding Various Path Planning Algorithms + - [x] Writing a brute Code for BFS + - [x] Using LIDAR to create a map and then use MOVE_BASE along with a custom GLOBAL PLANNER for BFS + - [x] Implement BFS in Simulation with Global/Static Map +* Stage 2: + - [ ] Mapping and Localization through OAK-D + - [ ] Implementing Algorithm for Dynamic Obstacle Avoidance in simulation + - [ ] Creating an Arena for the same + + + + +* Reference for the Model : [Robotis](https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/) We are Using turtle bot burger for our Path planning objective @@ -19,7 +45,7 @@ https://user-images.githubusercontent.com/97826285/219851215-2b4791e2-5c9c-4097- ---- + ## Algorithm for BFS ### Basic Algorithm for BFS @@ -32,27 +58,94 @@ nodes first, before moving to the next level neighbors. ![Algorithm Visualization](https://upload.wikimedia.org/wikipedia/commons/5/5d/Breadth-First-Search-Algorithm.gif) -## Pseudocode +## Pseudocode : Using Queues +`Note: That we use stack and layers instead of traditional queues to implement the BFS algorithm` ```sh BFS(root) + Pre: root is the node of the BST Post: the nodes in the BST have been visited in breadth first order q ← queue + while root = ø + yield root.value if root.left = ø q.enqueue(root.left) end if + if root.right = ø q.enqueue(root.right) end if + if !q.isEmpty() root ← q.dequeue() else root ← ø end if + + end while + +end BFS +``` +* Before Moving on to the next part we need to have a better understanding of three data structures + * **Vectors** : Vectors are sequence containers representing arrays that can change in size. [Click Here](https://cplusplus.com/reference/vector/vector/) to know more + * We don't specify namespace in the code because there are two main things `ros` and `std` so instead we use scope resolution operator to specify the functionality + * **Stacks** : Stacks are a type of container adaptors with LIFO(Last In First Out) type of working, where a new element is added at one end (top) and an element is removed from that end only. [Click here](https://www.geeksforgeeks.org/stack-in-cpp-stl/) to know more. + * **Pairs** : Pair is used to combine together two values that may be of different data types. Pair provides a way to store two heterogeneous objects as a single unit. It is basically used if we want to store tuples. The pair container is a simple container defined in < utility > header consisting of two data elements or objects. [Click here](https://www.geeksforgeeks.org/pair-in-cpp-stl/) to know more. + +## Pseudocode : Using Stacks + +Some Initializations: +* **path** : vector of pair, where two elements in the pair are the ith and jth index respectively +* **layer** : vector of pair of pairs, where one pair is for the parent node indices and the other pair is for child's node index +* **stack** : stack of vector of pair of pairs, basically used to store layers + +`Note : Layer contains the elements which lie in the same level.` +```sh +BFS(root) + + Pre: root is the node of the BST + Post: the nodes in the BST have been visited in breadth first order + st ← stack : which will store individual layers + layer ← layer : vector of pair of pairs, each pair contains pairs of indices of parent node and current node respectively + + // Creating a tree + while root != final index: + + check for the neighbours in east, south, north and west directions + layer = children which are unvisited, and then mark them visited + + st.push(layer) ← adding the whole layer + + clear layer ← After this new layer begins as the previous layer is already pushed into the stack and marked visited + end while + + if root == final index: + + path.push_back(final index) + while stack is not empty: + + temp_layer = st.top() ← get the topmost layer in the stack because it definitely will contain the final index + x = find final index in temp layer ← this will give us the node we were looking for + + if (found): + + parent = temp_layer[x].first ← Now get the parent of the and put that in the path + path.push_back(parent) ← parent is now in the path + + end if + + st.pop() + + end while + + end if + + return(path) + end BFS ``` @@ -63,7 +156,6 @@ end BFS * Testing bipartiteness of a graph. ----- ## Our use case #### There are two main steps that we need to perform here: - Storing each [node]() by connecting them to its neighbours which are unoccupied and unvisited. The subpart for this is: @@ -71,6 +163,56 @@ end BFS 2. Storing all the unvisited neighbours as children in the form of a list. - After storing, we just need to reach the required node, that will be set as target and will be given as the input by the user +## How to run BFS from this project + +1. In the first terminal source the cloning repo in the src folder of the workspace that you have created `git clone https://github.com/aPR0T0/TurtleBot-V3-BFS.git` + +``` + +nano ~/.bashrc +// And add these lines and the source the bashrc +alias get_tb3='source /opt/ros/noetic/setup.bash && export TURTLEBOT3_MODEL=burger && source ~/your_ws/devel/setup.bash' + +``` +2. As we have now created an alias so no need to repeat the previous step as you relaunch the nodes, just use the alias to source the directories! + +``` +// alias +get_tb3 + +roslaunch turtlebot3_gazebo turtlebot3_world.launch + +// In 3rd terminal +get_tb3 + +roslaunch turtlebot3_slam turtlebot3_slam slam_method:=gmapping + +// In 4th terminal +get_tb3 + +roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch + +// Now, just move the bot until whole arena is traversed and then close 4th terminal as map is not created +// Once done mapping close terminal 3 and go to next instruction + +// In 5th terminal +get_tb3 + +roslaunch turtlebot3 map_node.launch map_file:=$HOME/map.yaml + +// Now use Estimated pose Icon on GUI to give initial estimate of where bot is... +// then in 6th terminal +get_tb3 + +roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch + +// traverse the map until all particles closer and closer to the bot + +``` + +Voila! You got the simulation done!! +Now, Just click on nav_goal_2d in RVIZ GUI and see the magic of path planning + ## References - [trekhleb/javascript-algorithms](https://github.com/trekhleb/javascript-algorithms/tree/master/src/algorithms/tree/breadth-first-search) diff --git a/Test_djikstra/path_planning_intro.tar b/Test_djikstra/path_planning_intro.tar deleted file mode 100644 index f55ab46..0000000 Binary files a/Test_djikstra/path_planning_intro.tar and /dev/null differ diff --git a/Test_djikstra/turtlebot_description.tar b/Test_djikstra/turtlebot_description.tar deleted file mode 100644 index 887048a..0000000 Binary files a/Test_djikstra/turtlebot_description.tar and /dev/null differ diff --git a/pp_msgs/CMakeLists.txt b/pp_msgs/CMakeLists.txt new file mode 100644 index 0000000..2e62971 --- /dev/null +++ b/pp_msgs/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(pp_msgs) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +add_service_files( + FILES + PathPlanningPlugin.srv +) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES pp_msgs +# CATKIN_DEPENDS message_generation roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/pp_msgs.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/pp_msgs_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_pp_msgs.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/pp_msgs/package.xml b/pp_msgs/package.xml new file mode 100644 index 0000000..e7fcf31 --- /dev/null +++ b/pp_msgs/package.xml @@ -0,0 +1,28 @@ + + + pp_msgs + 0.1.0 + The pp_msgs package, for the mapping and path planning course + + Roberto Zegers + Roberto Zegers + BSD-3-Clause + + + + catkin + message_generation + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + message_runtime + + + + + + + diff --git a/pp_msgs/srv/PathPlanningPlugin.srv b/pp_msgs/srv/PathPlanningPlugin.srv new file mode 100644 index 0000000..6357702 --- /dev/null +++ b/pp_msgs/srv/PathPlanningPlugin.srv @@ -0,0 +1,7 @@ +int32[] costmap_ros +int32 width +int32 height +int32 start +int32 goal +--- +int32[] plan \ No newline at end of file diff --git a/scripts/.vscode/settings.json b/scripts/.vscode/settings.json deleted file mode 100644 index 1fb641e..0000000 --- a/scripts/.vscode/settings.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "python.autoComplete.extraPaths": [ - "/opt/ros/noetic/lib/python3/dist-packages" - ] -} \ No newline at end of file diff --git a/scripts/BFS.cpp b/scripts/BFS.cpp deleted file mode 100755 index 4e83fdb..0000000 --- a/scripts/BFS.cpp +++ /dev/null @@ -1,305 +0,0 @@ -#include "ros/ros.h" -#include "ros/console.h" -#include "std_msgs/String.h" -#include "nav_msgs/Odometry.h" -#include "tf/tf.h" -#include "std_msgs/Int8MultiArray.h" -#include "nav_msgs/OccupancyGrid.h" -#include "vector.h" -#include "bits/stdc++.h" - -int n1; -int m; -int curr_x, curr_y, curr_theta; -int des_x, des_y, des_theta; -int data1[9216]; - -long data_2d[96][96]; -int x, y, z, w; -std::queue> Que; -int visited[96][96]={-1}; // Marking all nodes as unvisited -// -1 -> univisited -// 1 -> visited - -//--------------------------------------------------------------------------// - /*Node for BFS*/ -class Node{ - public: - std::pair parent; // This stores index of the parent w.r.t 2d map obtained - std::vector> children; // This stores the indices of all the "unoccupied children" of the current Node - Node(){ - parent.first = -1; - parent.second = -1; - } - - // giving indices to the parent of the given class - Node(int i, int j){ - parent.first = i; - parent.second = j; - } - - void set_child(int i, int j){ - children.push_back(std::make_pair(i,j)); - } -}; -//--------------------------------------------------------------------------// - -//--------------------------------------------------------------------------// - /*Functions with one time use*/ - -void currentMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) -{ - - n1 = msg->info.width; - m = msg->info.height; - for(int i = 0 ; i < n1*m ; i++){ - - data1[i] = (msg->data[i]); - } - -} - -/* We need the odometry data to know where exactly are we present in the map*/ -void get_position(const nav_msgs::OdometryPtr& msg){ - curr_x = msg->pose.pose.position.x; - curr_y = msg->pose.pose.position.y; -} - -//--------------------------------------------------------------------------// - -void get_2d_map(int data1[]){ - for(int i = 0; i < n1 ; i++){ - for(int j = 0; j < (m-1) ; j++){ - data_2d[i][j] = data1[i*n1+j+1]; - } - } -} - - -//--------------------------------------------------------------------------// - - -//--------------------------------------------------------------------------// - /*Functions for distance and pixel indices conversion here*/ -std::pair distance_to_pixel(int distance_x, int distance_y){ - // 1 pixel = 0.1 meter - std::pair indices; - - indices.first = (distance_x + 5)/0.1 ;// origin of the map is position (-5,-5) - indices.second = (distance_y + 5)/0.1 ;// origin of the map is position (-5,-5) - - return indices; -} - -std::pair pixel_to_distance(int index_1, int index_2){ - std::pair pixels; - - pixels.first = index_1*0.1 - 5; - pixels.second = index_2*0.1 - 5; - - return pixels; -} -//--------------------------------------------------------------------------// - -//--------------------------------------------------------------------------// - - - /*BFS Function - Arguments: - Nodes - Returns: - Nothing - Functionality: - Makes the graph for the BFS - */ - - -void BFS_graph_builder(Node* &root, int i, int j){ - // Now we will be checking each free node which is in the- - // neighbourhood of range 1 pixel in the environment. - // if(i+1 > n1 || j+1 > m){ - //This condition will be used later - // } - visited[i][j] = 1; // Marking current node as visited too - - if(data_2d[i+1][j] == 0 && visited[i+1][j] == -1){ - //Setting a parent - Node* child = new Node({i+1, j}); - child->parent = {i, j}; - //Setting a Child - root->children.push_back({i+1,j}); - Que.push({i+1,j}); - visited[i+1][j] = 1; - } - - if(data_2d[i][j+1] == 0 && visited[i][j+1] == -1){ - - Node* child = new Node({i, j+1}); - child->parent = {i, j}; - - root->children.push_back({i,j+1}); - Que.push({i,j+1}); - visited[i][j+1] = 1; - } - - if(data_2d[i+1][j+1] == 0 && visited[i+1][j+1] == -1){ - - Node* child = new Node({i+1, j+1}); - child->parent = {i, j}; - - root->children.push_back({i+1,j+1}); - Que.push({i+1,j+1}); - visited[i+1][j+1] = 1; - } - - if(data_2d[i-1][j+1] == 0 && visited[i][j+1] == -1){ - - Node* child = new Node({i-1, j+1}); - child->parent = {i, j}; - - root->children.push_back({i-1,j+1}); - Que.push({i-1,j+1}); - visited[i][j+1] = 1; - } - - if(data_2d[i-1][j] == 0 && visited[i-1][j] == -1){ - - Node* child = new Node({i-1, j}); - child->parent = {i, j}; - - root->children.push_back({i-1,j}); - Que.push({i-1,j}); - visited[i-1][j] = 1; - } - - if(data_2d[i][j-1] == 0 && visited[i][j-1] == -1){ - - Node* child = new Node({i, j-1}); - child->parent = {i, j}; - - root->children.push_back({i,j-1}); - Que.push({i,j-1}); - visited[i][j-1] = 1; - } - - if(data_2d[i-1][j-1] == 0 && visited[i-1][j-1] == -1){ - - Node* child = new Node({i-1, j-1}); - child->parent = {i, j}; - - root->children.push_back({i-1,j-1}); - Que.push({i-1,j-1}); - visited[i-1][j-1] = 1; - } - - if(data_2d[i+1][j-1] == 0 && visited[i+1][j-1] == -1){ - - Node* child = new Node({i+1, j-1}); - child->parent = {i, j}; - - root->children.push_back({i+1,j-1}); - Que.push({i+1,j-1}); - visited[i+1][j-1] = 1; - } -} -//--------------------------------------------------------------------------// - -//--------------------------------------------------------------------------// - // ## Returning Path ## // -std::vector> return_path(std::pair final_index, std::pair initial_index, Node* root){ - std::vector> path; - std::pair parentx; - - if(final_index.first == initial_index.first && final_index.second == initial_index.second){ - path.push_back(initial_index); - return path; - } - else if(final_index.first < 0 && final_index.second < 0){ - path.clear(); - path.push_back({-1,-1}); - return path; - } - parentx = root->parent; - root = new Node(root->parent.first, root->parent.second); // Now going back to the parent - path.push_back(parentx); - return_path(final_index, parentx, root); -} -//--------------------------------------------------------------------------// - -int main(int argc, char **argv) { - ros::init(argc, argv, "map_interpretor"); - - ros::NodeHandle n; - // Subscribing to map topic where all the data of the map is coming from - ros::Subscriber subscriber = n.subscribe("map",1000, currentMap); - // Subscribing to odometry topic to get the pose estimate - ros::Subscriber odo_sub = n.subscribe("odom", 1000, get_position); - ros::Rate rate(10); - - int count = 0; - while (ros::ok()) - { - ros::Duration current_time(ros::Time::now().toSec()); /* current time*/ - get_2d_map(data1); //got the 2d map to operate upon - - // Initializing the node root which is going to be the first and the only node with no parent - std::pair u, v; // pixels of the current grid cell - std::vector> path; - - u = distance_to_pixel(curr_x, curr_y); - v = distance_to_pixel(des_x, des_y); - Node* root = new Node(u.first, u.second); - // Marking current node as visited - -//--------------------------------------------------------------------------// - /*BFS Function declared Up there - Arguments: - Nodes - Returns: - Nothing - Functionality: - Makes the graph for the BFS - */ - BFS_graph_builder(root, u.first, u.second); - // Now all the neighbouring nodes are added to the children vector in the current node - - // Now let's check if the queue is empty or not - if(!Que.empty()){ - - std::pair indx = Que.front(); - Que.pop(); // Now marking current element as visited - - if(indx.first == v.first && indx.second == v.second){// This means the goal is reached - path = return_path(v, u, root); - } - - else if(indx.first > v.first && indx.second > v.second){ - path = return_path({-1,-1}, u, root); // This will return no path - } - - Node* root = new Node(indx.first, indx.second);// Now going to the First Child - - BFS_graph_builder(root, indx.first, indx.second); // Now building the graph further - - } - -//--------------------------------------------------------------------------// - - ros::spinOnce(); - - rate.sleep(); - ++count; - } - - - return 0; -} - - -/* -// - - ####################################### Algorithm for the Code ##################################### - -// -*/ \ No newline at end of file diff --git a/scripts/controller.cpp b/scripts/controller.cpp deleted file mode 100755 index 4f80b40..0000000 --- a/scripts/controller.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include "geometry_msgs/Twist.h" -#include "nav_msgs/Odometry.h" -#include "tf/tf.h" -#include "math.h" -#include - -/** - * This tutorial demonstrates simple sending of messages over the ROS system. - */ -int desired_x[5],desired_y[5]; -int turtle_x, turtle_y, yaw[5]; -tfScalar roll, pitch, turtle_theta; -int D; // Extended Diameter -int counter = 0; // Counter so that we take the first reading as the global frame readings -int k_x, k_y, k_theta; - -k_x = 1; -k_y = 1; -k_theta = 1; - -void callback(const nav_msgs::OdometryConstPtr& msg){ // we need to make a pointer for each message type in roscpp - - turtle_x = msg.pose.pose.position.desired_x; - turtle_y = msg.pose.pose.position.desired_y; - tf::Matrix3x3 mat(msg.pose.pose.orientation); - mat.getEulerYPR(&turtle_theta, &pitch, &roll); - -} - -int main(int argc, char **argv){ - - ros::init(argc, argv, "controller"); - - ros::NodeHandle n; - - ros::Publisher velocity_pub = n.advertise("Command_vel", 1000); - - ros::Rate rate(10); - - ros::Subscriber subscriber = n.subscribe("odo_sub", 10, callback); - - /** - * A count of how many messages we have sent. This is used to create - * a unique string for each message. - */ - int count = 0; - int t = 0; - int err_x, err_y, err_theta; - - while (ros::ok()) - { - err_x = desired_x[t] - turtle_x; - err_y = desired_y[t] - turtle_y; - err_theta = yaw[t] - turtle_theta; - /** - * This is a message object. You stuff it with data, and then publish it. - */ - // std_msgs::String msg;// No specific standard is provided because we are using two standards here one is ros and another is standard - geometry_msgs::Twist velocity; - - if(0.01 >= err_x > -0.01 and -0.01<= err_y <= 0.01 and -0.01 <= err_theta < 0.01){ - - } - // ROS_INFO("%s", msg.data.c_str()); - - /** - * The publish() function is how you send messages. The parameter - * is the message object. The type of this object must agree with the type - * given as a template parameter to the advertise<>() call, as was done - * in the constructor above. - */ - velocity_pub.publish(velocity); - - ros::spinOnce(); - - rate.sleep(); - ++count; - } - - - return 0; -} \ No newline at end of file diff --git a/scripts/costmap_plugin.xml b/scripts/costmap_plugin.xml deleted file mode 100644 index cf91a7c..0000000 --- a/scripts/costmap_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Demo Layer that marks all points that were ever one meter in front of the robot - - - \ No newline at end of file diff --git a/scripts/include/simple_layer.h b/scripts/include/simple_layer.h deleted file mode 100644 index 46457ed..0000000 --- a/scripts/include/simple_layer.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef GRID_LAYER_H_ -#define GRID_LAYER_H_ -#include -#include -#include -#include -#include - -namespace simple_layer_namespace -{ - -class GridLayer : public costmap_2d::Layer, public costmap_2d::Costmap2D -{ -public: - GridLayer(); - - virtual void onInitialize(); - virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y); - virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); - bool isDiscretized() - { - return true; - } - - virtual void matchSize(); - -private: - void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level); - dynamic_reconfigure::Server *dsrv_; -}; -} -#endif \ No newline at end of file diff --git a/scripts/listener.cpp b/scripts/listener.cpp deleted file mode 100755 index 23e6324..0000000 --- a/scripts/listener.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include "nav_msgs/OccupancyGrid.h" - -/** - * This tutorial demonstrates simple receipt of messages over the ROS system. - */ -void chatterCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg) -{ - ROS_INFO("I heard: [%d]", msg->data); -} - -int main(int argc, char **argv) -{ - /** - * The ros::init() function needs to see argc and argv so that it can perform - * any ROS arguments and name remapping that were provided at the command line. - * For programmatic remappings you can use a different version of init() which takes - * remappings directly, but for most command-line programs, passing argc and argv is - * the easiest way to do it. The third argument to init() is the name of the node. - * - * You must call one of the versions of ros::init() before using any other - * part of the ROS system. - */ - ros::init(argc, argv, "listener"); - - /** - * NodeHandle is the main access point to communications with the ROS system. - * The first NodeHandle constructed will fully initialize this node, and the last - * NodeHandle destructed will close down the node. - */ - ros::NodeHandle n; - - /** - * The subscribe() call is how you tell ROS that you want to receive messages - * on a given topic. This invokes a call to the ROS - * master node, which keeps a registry of who is publishing and who - * is subscribing. Messages are passed to a callback function, here - * called chatterCallback. subscribe() returns a Subscriber object that you - * must hold on to until you want to unsubscribe. When all copies of the Subscriber - * object go out of scope, this callback will automatically be unsubscribed from - * this topic. - * - * The second parameter to the subscribe() function is the size of the message - * queue. If messages are arriving faster than they are being processed, this - * is the number of messages that will be buffered up before beginning to throw - * away the oldest ones. - */ - ros::Subscriber sub = n.subscribe("map", 1000, chatterCallback); - - /** - * ros::spin() will enter a loop, pumping callbacks. With this version, all - * callbacks will be called from within this thread (the main one). ros::spin() - * will exit when Ctrl-C is pressed, or the node is shutdown by the master. - */ - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/scripts/map b/scripts/map deleted file mode 100755 index 741db4d..0000000 Binary files a/scripts/map and /dev/null differ diff --git a/scripts/map.cpp b/scripts/map.cpp deleted file mode 100644 index 6258e72..0000000 --- a/scripts/map.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include "nav_msgs/OccupancyGrid.h" - -int n1; -int m; -int data[147456]; -int x, y, z, w; -void currentMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) -{ - - n1 = msg->info.width; - m = msg->info.height; - for(int i = 0 ; i < n1 ; i++){ - data[i] = msg->data[i]; - } -} - -int main(int argc, char **argv) { - ros::init(argc, argv, "map_interpretor"); - - ros::NodeHandle n; - - int data[147456]; - ros::Subscriber subscriber = n.subscribe("map",1000, currentMap); - ros::Rate rate(10); - - int count = 0; - while (ros::ok()) - { - ros::Duration current_time(ros::Time::now().toSec()); /* current time*/ - int data_2d[n1][m]; - - for(int i = 0; i < n1 ; i++){ - for(int j = 0; j < m ; j++){ - data_2d[i][j] = data[j]; - } - } - - for(int i = 0; i < n1; i++){ - - for(int j = 0; j < m; j++){ - ROS_INFO("READING: %d", data_2d[i][j]); - } - - } - ros::spinOnce(); - - rate.sleep(); - ++count; - } - - - return 0; -} \ No newline at end of file diff --git a/scripts/src/Costmap2d.cpp b/scripts/src/Costmap2d.cpp deleted file mode 100755 index a8801e6..0000000 --- a/scripts/src/Costmap2d.cpp +++ /dev/null @@ -1,77 +0,0 @@ -#include -#include - -PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer) - -using costmap_2d::LETHAL_OBSTACLE; -using costmap_2d::NO_INFORMATION; - -namespace simple_layer_namespace -{ - -GridLayer::GridLayer() {} - -void GridLayer::onInitialize() -{ - ros::NodeHandle nh("~/" + name_); - current_ = true; - default_value_ = NO_INFORMATION; - matchSize(); - - dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = boost::bind( - &GridLayer::reconfigureCB, this, _1, _2); - dsrv_->setCallback(cb); -} - - -void GridLayer::matchSize() -{ - Costmap2D* master = layered_costmap_->getCostmap(); - resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), - master->getOriginX(), master->getOriginY()); -} - - -void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level) -{ - enabled_ = config.enabled; -} - -void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y) -{ - if (!enabled_) - return; - - double mark_x = robot_x + cos(robot_yaw), mark_y = robot_y + sin(robot_yaw); - unsigned int mx; - unsigned int my; - if(worldToMap(mark_x, mark_y, mx, my)){ - setCost(mx, my, LETHAL_OBSTACLE); - } - - *min_x = std::min(*min_x, mark_x); - *min_y = std::min(*min_y, mark_y); - *max_x = std::max(*max_x, mark_x); - *max_y = std::max(*max_y, mark_y); -} - -void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, - int max_j) -{ - if (!enabled_) - return; - - for (int j = min_j; j < max_j; j++) - { - for (int i = min_i; i < max_i; i++) - { - int index = getIndex(i, j); - if (costmap_[index] == NO_INFORMATION) - continue; - master_grid.setCost(i, j, costmap_[index]); - } - } -} - -} // end namespace \ No newline at end of file diff --git a/scripts/src/bfs.cpp b/scripts/src/bfs.cpp deleted file mode 100755 index 400150e..0000000 --- a/scripts/src/bfs.cpp +++ /dev/null @@ -1,353 +0,0 @@ -#include "ros/ros.h" -#include "ros/console.h" -#include "std_msgs/String.h" -#include "nav_msgs/Odometry.h" -#include "tf/tf.h" -#include "std_msgs/Int8MultiArray.h" -#include "geometry_msgs/Point.h" -#include "nav_msgs/OccupancyGrid.h" -#include -#include "bits/stdc++.h" - -int n1; -int m; -int curr_x, curr_y, curr_theta; -int des_x, des_y, des_theta; -int data1[9216]; -const int N = 1e3; -long data_2d[96][96]; -int x, y, z, w; -std::queue> Que; -int visited[1000][1000]={-1}; // Marking all nodes as unvisited -// -1 -> univisited -// 1 -> visited - -//--------------------------------------------------------------------------// - /*Node for BFS*/ -class Node{ - public: - std::pair parent; // This stores index of the parent w.r.t 2d map obtained - std::vector> children; // This stores the indices of all the "unoccupied children" of the current Node - Node(){ - parent.first = -1; - parent.second = -1; - } - - // giving indices to the parent of the given class - Node(int i, int j){ - parent.first = i; - parent.second = j; - } - - void set_child(int i, int j){ - children.push_back(std::make_pair(i,j)); - } -}; -//--------------------------------------------------------------------------// - -//--------------------------------------------------------------------------// - /*Functions with one time use*/ - -void currentMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) -{ - - n1 = msg->info.width; - m = msg->info.height; - for(int i = 0 ; i < n1*m ; i++){ - - data1[i] = (msg->data[i]); - } - -} - -/* We need the odometry data to know where exactly are we present in the map*/ -void get_position(const nav_msgs::OdometryPtr& msg){ - curr_x = msg->pose.pose.position.x; - curr_y = msg->pose.pose.position.y; -} - -//--------------------------------------------------------------------------// - -void get_2d_map(int data1[]){ - for(int i = 0; i < n1 ; i++){ - for(int j = 0; j < (m-1) ; j++){ - data_2d[i][j] = data1[i*n1+j+1]; - } - } -} - - -//--------------------------------------------------------------------------// - - -//--------------------------------------------------------------------------// - /*Functions for distance and pixel indices conversion here*/ -std::pair distance_to_pixel(int distance_x, int distance_y){ - // 1 pixel = 0.1 meter - std::pair indices; - - indices.first = (distance_x + 5)/0.1 ;// origin of the map is position (-5,-5) - indices.second = (distance_y + 5)/0.1 ;// origin of the map is position (-5,-5) - - return indices; -} - -std::pair pixel_to_distance(int index_1, int index_2){ - std::pair pixels; - - pixels.first = index_1*0.1 - 5; - pixels.second = index_2*0.1 - 5; - - return pixels; -} -//--------------------------------------------------------------------------// - -//--------------------------------------------------------------------------// - - - /*BFS Function - Arguments: - Nodes - Returns: - Nothing - Functionality: - Makes the graph for the BFS - */ - - -void BFS_graph_builder(Node* &root, int i, int j){ - // Now we will be checking each free node which is in the- - // neighbourhood of range 1 pixel in the environment. - visited[i][j] = 1; // Marking current node as visited too - - if(data_2d[i+1][j] == 0 && visited[i+1][j] == -1){ - - if( i+1 < n1 ){ // Seeing whether the index is even feasible or not - //Setting a parent - Node* child = new Node({i+1, j}); - child->parent = {i, j}; - //Setting a Child - root->children.push_back({i+1,j}); - Que.push({i+1,j}); - visited[i+1][j] = 1; - } - - } - - if(data_2d[i][j+1] == 0 && visited[i][j+1] == -1){ - if( j+1 < m ){ - Node* child = new Node({i, j+1}); - child->parent = {i, j}; - - root->children.push_back({i,j+1}); - Que.push({i,j+1}); - visited[i][j+1] = 1; - } - } - - if(data_2d[i+1][j+1] == 0 && visited[i+1][j+1] == -1){ - - if( i+1 < n1 and j+1 < m ){ - Node* child = new Node({i+1, j+1}); - child->parent = {i, j}; - - root->children.push_back({i+1,j+1}); - Que.push({i+1,j+1}); - visited[i+1][j+1] = 1; - } - - } - - if(data_2d[i-1][j+1] == 0 && visited[i][j+1] == -1){ - - if( i-1 >= 0 and j+1 < m){ - Node* child = new Node({i-1, j+1}); - child->parent = {i, j}; - - root->children.push_back({i-1,j+1}); - Que.push({i-1,j+1}); - visited[i][j+1] = 1; - } - - } - - if(data_2d[i-1][j] == 0 && visited[i-1][j] == -1){ - if( i-1 >= 0 ){ - Node* child = new Node({i-1, j}); - child->parent = {i, j}; - - root->children.push_back({i-1,j}); - Que.push({i-1,j}); - visited[i-1][j] = 1; - } - } - - if(data_2d[i][j-1] == 0 && visited[i][j-1] == -1){ - if( j >= 0 ){ - Node* child = new Node({i, j-1}); - child->parent = {i, j}; - - root->children.push_back({i,j-1}); - Que.push({i,j-1}); - visited[i][j-1] = 1; - } - } - - if(data_2d[i-1][j-1] == 0 && visited[i-1][j-1] == -1){ - if(i-1 >= 0 and j-1 >= 0){ - Node* child = new Node({i-1, j-1}); - child->parent = {i, j}; - - root->children.push_back({i-1,j-1}); - Que.push({i-1,j-1}); - visited[i-1][j-1] = 1; - } - } - - if(data_2d[i+1][j-1] == 0 && visited[i+1][j-1] == -1){ - if( i+1 < n1 and j-1 >= 0 ){ - Node* child = new Node({i+1, j-1}); - child->parent = {i, j}; - - root->children.push_back({i+1,j-1}); - Que.push({i+1,j-1}); - visited[i+1][j-1] = 1; - } - } -} -//--------------------------------------------------------------------------// - -//--------------------------------------------------------------------------// - // ## Returning Path ## // -std::vector> return_path(std::pair final_index, std::pair initial_index, Node* root){ - std::vector> path; - std::pair parentx; - - if(final_index.first == initial_index.first && final_index.second == initial_index.second){ - path.push_back(initial_index); - return path; - } - else if(final_index.first < 0 && final_index.second < 0){ - path.clear(); - path.push_back({-1,-1}); - return path; - } - parentx = root->parent; - root = new Node(root->parent.first, root->parent.second); // Now going back to the parent - path.push_back(parentx); - return_path(final_index, parentx, root); -} -//--------------------------------------------------------------------------// - -int main(int &argc, char **argv) { - ros::init(argc, argv, "map_interpretor"); - - ros::NodeHandle n; - // Subscribing to map topic where all the data of the map is coming from - ros::Subscriber subscriber = n.subscribe("map",1000, currentMap); - // Subscribing to odometry topic to get the pose estimate - ros::Subscriber odo_sub = n.subscribe("odom", 1000, get_position); - ros::Publisher path_publisher = n.advertise("path_sub", 1); - ros::Rate rate(10); - - int count = 0; - while (ros::ok()) - { - ros::Duration current_time(ros::Time::now().toSec()); /* current time*/ - get_2d_map(data1); //got the 2d map to operate upon - - // Initializing the node root which is going to be the first and the only node with no parent - std::pair u, v; // pixels of the current grid cell - std::vector> path; // This is going to be the path we traversed - scripts::Points msg; // This is going to be the final published path - geometry_msgs::Point path_pub[N]; - geometry_msgs::Point point_pub; - - u = distance_to_pixel(curr_x, curr_y); - v = distance_to_pixel(des_x, des_y); - Node* root = new Node(u.first, u.second); - // Marking current node as visited - -//--------------------------------------------------------------------------// - /*BFS Function declared Up there - Arguments: - Nodes - Returns: - Nothing - Functionality: - Makes the graph for the BFS - */ - BFS_graph_builder(root, u.first, u.second); - // Now all the neighbouring nodes are added to the children vector in the current node - - // Now let's check if the queue is empty or not - while(!Que.empty() && path.empty()){ - - std::pair indx = Que.front(); - Que.pop(); // Now marking current element as visited - - if(indx.first == v.first && indx.second == v.second){// This means the goal is reached - path = return_path(v, u, root); - for(int i = 0; i < path.size(); i++){ - point_pub.x = path[i].first; - point_pub.y = path[i].second; - path_pub[i] = point_pub; - }// the path we just got is in terms of indices so we need to convert them to distances as we need to give waypoints to the controller - } - - else if(indx.first > v.first && indx.second > v.second){ - path = return_path({-1,-1}, u, root); // This will return no path - } - - Node* root = new Node(indx.first, indx.second);// Now going to the First Child - - BFS_graph_builder(root, indx.first, indx.second); // Now building the graph further - - } - // Now as there is some path existing - // We now need to get the path in form of distances - for(int i = 0; i < path.size() ; i++){ - std::pair path_temp; - point_pub = path_pub[i]; - path_temp = pixel_to_distance (point_pub.x, point_pub.y); - - point_pub.x = path_temp.first; - point_pub.y = path_temp.second; - - path_pub[i] = point_pub; - } -//-----------------------------Now Just publish the Path you just obtained---------------------------------// - std::vector my_vector (path_pub, path_pub + sizeof(path_pub) / sizeof(geometry_msgs::Point)); - - msg.points.clear(); - msg.end_index = path.size() - 1; - int index_z = 0; - // Now declaring the messages with the help of the points - for (std::vector::iterator it = my_vector.begin(); it != my_vector.end(); ++it) { - geometry_msgs::Point point; - point.x = (*it).x; - point.y = (*it).y; - point.z = 0; - msg.points.push_back(point); - index_z++; - } - - path_publisher.publish(msg); - ros::spinOnce(); - - rate.sleep(); - ++count; - } - - - return 0; -} - - -/* -// - - ####################################### Algorithm for the Code ##################################### - -// -*/ \ No newline at end of file diff --git a/scripts/src/bfs_test_small_matrix.cpp b/scripts/src/bfs_test_small_matrix.cpp deleted file mode 100644 index aec5101..0000000 --- a/scripts/src/bfs_test_small_matrix.cpp +++ /dev/null @@ -1,288 +0,0 @@ -#include "ros/ros.h" -#include "ros/console.h" -#include "std_msgs/String.h" -#include "std_msgs/Int8MultiArray.h" -#include "bits/stdc++.h" - -int n1 = 5; -int m = 5; -float des_x, des_y, des_theta; -int data1[25] = {1,0,0,0,0,1,0,0,0,0,0,0,1,1,0,1,0,0,0,0,0,0,0,0,0}; -const int N = 1e2; -long data_2d[5][5]; -std::vector> path; -int x, y, z, w; -std::queue> Que; -int visited[N][N] = {0}; // Marking all nodes as unvisited -// -1 -> univisited -// 1 -> visited - -//--------------------------------------------------------------------------// - /*Node for BFS*/ -class Node{ - public: - std::pair its_indx; - std::pair parent; // This stores index of the parent w.r.t 2d map obtained - std::vector> children; // This stores the indices of all the "unoccupied children" of the current Node - - // giving indices to the parent of the given class - - Node(int i, int j){ - its_indx.first = i; - its_indx.second = j; - } - - Node(int i, int j, int i1, int j1){ - its_indx.first = i1; - its_indx.second = j1; - parent.first = i; // initializing as parent and children still null - parent.second = j; // initializing as parent and children still null - // children.clear(); // When initializing a new node there is no child - } - - void set_child(int i, int j){ - children.push_back(std::make_pair(i,j)); - } -}; -//--------------------------------------------------------------------------// -//--------------------------------------------------------------------------// - -void get_2d_map(int data1[]){ - for(int i = 0; i < n1 ; i++){ - for(int j = 0; j < m ; j++){ - data_2d[i][j] = data1[i*n1+j]; - } - } -} - - -//--------------------------------------------------------------------------// - - -//--------------------------------------------------------------------------// - /*Functions for distance and pixel indices conversion here*/ -std::pair distance_to_pixel(float distance_x, float distance_y){ - // 1 pixel = 0.1 meter - std::pair indices; - // std::cout< pixel_to_distance(float index_1, float index_2){ - std::pair pixels; - - pixels.first = index_1*0.1 - 0.25; // x - axis - pixels.second = index_2*0.1 - 0.25; // y - axis - - return pixels; -} -//--------------------------------------------------------------------------// - -//--------------------------------------------------------------------------// - - - /*BFS Function - Arguments: - Nodes - Returns: - Nothing - Functionality: - Makes the graph for the BFS - */ - - -void BFS_graph_builder(Node* &root, int i, int j){ - // Now we will be checking each free node which is in the- - // neighbourhood of range 1 pixel in the environment. - visited[i][j] = 1; // Marking current node as visited too - - if(data_2d[i+1][j] == 0 && visited[i+1][j] == 0){ - - if( i+1 < n1 ){ // Seeing whether the index is even feasible or not - //Setting a parent - Node* child = new Node(i,j,i+1, j); - root->children.push_back({i+1,j}); - Que.push({i+1,j}); - visited[i+1][j] = 1; - } - - } - - if(data_2d[i][j+1] == 0 && visited[i][j+1] == 0){ - - if( j+1 < m ){ - Node* child = new Node(i,j,i, j+1); - - root->children.push_back({i,j+1}); - Que.push({i,j+1}); - visited[i][j+1] = 1; - } - } - - if(data_2d[i+1][j+1] == 0 && visited[i+1][j+1] == 0){ - - if( i+1 < n1 and j+1 < m ){ - Node* child = new Node(i,j,i+1, j+1); - root->children.push_back({i+1,j+1}); - Que.push({i+1,j+1}); - visited[i+1][j+1] = 1; - } - - } - - if(data_2d[i-1][j+1] == 0 && visited[i][j+1] == 0){ - - if( i-1 >= 0 and j+1 < m){ - Node* child = new Node(i,j,i-1, j+1); - root->children.push_back({i-1,j+1}); - Que.push({i-1,j+1}); - visited[i][j+1] = 1; - } - - } - - if(data_2d[i-1][j] == 0 && visited[i-1][j] == 0){ - - if( i-1 >= 0 ){ - Node* child = new Node(i,j,i-1, j); - root->children.push_back({i-1,j}); - Que.push({i-1,j}); - visited[i-1][j] = 1; - } - } - - if(data_2d[i][j-1] == 0 && visited[i][j-1] == 0){ - - if( j >= 0 ){ - Node* child = new Node(i,j,i, j-1); - - root->children.push_back({i,j-1}); - Que.push({i,j-1}); - visited[i][j-1] = 1; - } - } - - if(data_2d[i-1][j-1] == 0 && visited[i-1][j-1] == 0){ - - if(i-1 >= 0 and j-1 >= 0){ - Node* child = new Node(i,j,i-1, j-1); - - root->children.push_back({i-1,j-1}); - Que.push({i-1,j-1}); - visited[i-1][j-1] = 1; - } - } - - if(data_2d[i+1][j-1] == 0 && visited[i+1][j-1] == 0){ - - if( i+1 < n1 and j-1 >= 0 ){ - Node* child = new Node(i,j,i+1, j-1); - root->children.push_back({i+1,j-1}); - Que.push({i+1,j-1}); - visited[i+1][j-1] = 1; - } - } -} -//--------------------------------------------------------------------------// - -//--------------------------------------------------------------------------// - // ## Returning Path ## // -std::vector> return_path(std::pair curr_index, std::pair initial_index, Node* &root){ - std::pair indices = root->parent; - if(curr_index == initial_index){ - path.push_back({indices.first,indices.second}); - return path; - } - - if(curr_index.first < 0 and curr_index.second < 0){ - path.clear(); - return path; - } - - path.push_back({indices.first,indices.second}); - curr_index = indices; - root = new Node(curr_index.first, curr_index.second); - path = return_path(curr_index, initial_index, root); -} -//--------------------------------------------------------------------------// - -int main() { - - int count = 0; - // ros::Duration current_time(ros::Time::now().toSec()); /* current time*/ - > As such no need for this at this point of time - get_2d_map(data1); //got the 2d map to operate upon - - for(int i = 0; i < n1 ; i++){ - for(int j = 0; j < m ; j++){ - std::cout< u; - std::pair v; // pixels of the current grid cell - std::vector> path; // This is going to be the path we traversed - - std::cout<<"Enter the values for desired coordinates : x y and orientation : Theta "; - std::cin>>des_x>>des_y>>des_theta; - - u = distance_to_pixel(curr_x, curr_y); - v = distance_to_pixel(des_x, des_y); - - Node* root = new Node(-1,-1,u.first,u.second); // Initial root cannot have a parent - // Marking current node as visited - visited[u.first][u.second] = 1; - Que.push({u.first,u.second}); - -//--------------------------------------------------------------------------// - /*BFS Function declared Up there - Arguments: - Nodes - Returns: - Nothing - Functionality: - Makes the graph for the BFS - */ - - BFS_graph_builder(root, u.first, u.second); // Single layer has been built - // Now all the neighbouring nodes are added to the children vector in the current node - // Now let's check if the queue is empty or not - if(Que.empty()){ - std::cout<<"While loop not working"; - } - while(!Que.empty() && path.empty()){ - - std::pair indx = Que.front(); - - Que.pop(); // Now marking current element as visited - - if(indx.first == v.first && indx.second == v.second){// This means the goal is reached - std::cout<< "Path was found"< v.first && indx.second > v.second){ - return_path({-1,-1}, u, root); // This will return no path - std::cout<< "No Path was found"<parent.first,root->parent.second,indx.first, indx.second);// Now going to the First Child - - BFS_graph_builder(root, indx.first, indx.second); // Now building the graph further - - } - return 0; -} diff --git a/scripts/src/controller.cpp b/scripts/src/controller.cpp deleted file mode 100755 index 7c74aea..0000000 --- a/scripts/src/controller.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include "geometry_msgs/Twist.h" -#include "geometry_msgs/Pose2D.h" -#include "nav_msgs/Odometry.h" -#include -#include "tf/tf.h" -#include "math.h" -#include - -/** - * This tutorial demonstrates simple sending of messages over the ROS system. - */ -double desired_x[5],desired_y[5], yaw[5]; -double turtle_x, turtle_y; -double prev_x = 0, prev_y = 0; -double roll, pitch, turtle_theta; -int D; // Extended Diameter -int counter = 0; // Counter so that we take the first reading as the global frame readings -const int k_x = 1, k_y = 1, k_theta = 1; -int index_x = 0; -void callback(const scripts::Points::ConstPtr& msg ){ // we need to make a pointer for each message type in roscpp - - desired_x[index_x] = msg->points[index_x].x; - desired_y[index_x] = msg->points[index_x].y; - - yaw[index_x] = std::atan2( ( turtle_x - prev_x ),( turtle_y - prev_y) ); - -} - -void callback_odo(const nav_msgs::Odometry::ConstPtr& msg ){ - turtle_x = msg->pose.pose.position.x; - turtle_y = msg->pose.pose.position.y; - - tf::Quaternion q( - // Does nothing but a way to store a quaternion - msg->pose.pose.orientation.w, - msg->pose.pose.orientation.x, - msg->pose.pose.orientation.y, - msg->pose.pose.orientation.z - ); - - tf::Matrix3x3(q).getRPY(roll,pitch,turtle_theta); - // gets orientation in RPY for the given quaternions -} - -int main(int argc, char **argv){ - - ros::init(argc, argv, "controller"); - - ros::NodeHandle n; - - ros::Publisher velocity_pub = n.advertise("Command_vel", 1000); - - ros::Rate rate(10); - - ros::Subscriber subscriber = n.subscribe("path_sub", 10, callback); - ros::Subscriber subs = n.subscribe("odo_sub", 10, callback_odo); - - /** - * A count of how many messages we have sent. This is used to create - * a unique string for each message. - */ - int count = 0; - int t = 0; - int err_x, err_y, err_theta; - while (ros::ok()) - { - err_x = desired_x[t] - turtle_x; - err_y = desired_y[t] - turtle_y; - err_theta = yaw[t] - turtle_theta; - /** - * This is a message object. You stuff it with data, and then publish it. - */ - // std_msgs::String msg;// No specific standard is provided because we are using two standards here one is ros and another is standard - geometry_msgs::Twist velocity; - - if(0.01 >= err_x > -0.01 and -0.01<= err_y <= 0.01 and -0.01 <= err_theta < 0.01){ - index_x++; - // The subscription is now again called for the next index until the final index is reached - ros::Subscriber subscriber = n.subscribe("path_sub", 10, callback); - ros::Subscriber subs = n.subscribe("odo_sub", 10, callback_odo); - } - // ROS_INFO("%s", msg.data.c_str()); - - /** - * The publish() function is how you send messages. The parameter - * is the message object. The type of this object must agree with the type - * given as a template parameter to the advertise<>() call, as was done - * in the constructor above. - */ - - velocity_pub.publish(velocity); - - ros::spinOnce(); - - rate.sleep(); - ++count; - } - - - return 0; -} \ No newline at end of file diff --git a/scripts/.vscode/c_cpp_properties.json b/turtlebot3/.vscode/c_cpp_properties.json similarity index 100% rename from scripts/.vscode/c_cpp_properties.json rename to turtlebot3/.vscode/c_cpp_properties.json diff --git a/turtlebot3/.vscode/settings.json b/turtlebot3/.vscode/settings.json new file mode 100644 index 0000000..b4159c4 --- /dev/null +++ b/turtlebot3/.vscode/settings.json @@ -0,0 +1,86 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csetjmp": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "cuchar": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "scoped_allocator": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp", + "*.ipp": "cpp", + "core": "cpp" + } +} \ No newline at end of file diff --git a/scripts/CMakeLists.txt b/turtlebot3/CMakeLists.txt similarity index 88% rename from scripts/CMakeLists.txt rename to turtlebot3/CMakeLists.txt index 41c07a3..4d51486 100644 --- a/scripts/CMakeLists.txt +++ b/turtlebot3/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(scripts) +project(turtlebot3) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -8,10 +8,17 @@ project(scripts) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + actionlib + nav_core + nav_msgs + tf + base_local_planner + move_base_msgs costmap_2d dynamic_reconfigure roscpp std_msgs + pp_msgs message_generation ) @@ -105,12 +112,11 @@ generate_messages( ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES my_pkg + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS geometry_msgs roscpp std_msgs message_runtime # DEPENDS system_lib ) - ########### ## Build ## ########### @@ -122,10 +128,10 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -add_library(scripts src/Costmap2d.cpp) - +## Declare a C++ library +add_library(global_planner_lib + src/global_planner.cpp +) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure @@ -134,24 +140,26 @@ add_library(scripts src/Costmap2d.cpp) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -add_executable(map_node map.cpp) +add_executable(bfs_server src/bfs_server.cpp) add_executable(bfs src/bfs.cpp) -add_executable(controller src/controller.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") +# set_target_properties(bfs_server PROPERTIES LINKER_LANGUAGE FIXED) + +# set_target_properties(bfs PROPERTIES LINKER_LANGUAGE FIXED) + +# set_target_properties(controller PROPERTIES LINKER_LANGUAGE FIXED) ## Add cmake target dependencies of the executable ## same as for the library above -add_dependencies(map_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(bfs_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(bfs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -target_link_libraries(map_node +target_link_libraries(bfs_server ${catkin_LIBRARIES} ) target_link_libraries(bfs @@ -159,10 +167,6 @@ target_link_libraries(bfs ${catkin_LIBRARIES} ) -target_link_libraries(controller - ${catkin_LIBRARIES} -) - ############# ## Install ## ############# @@ -179,17 +183,14 @@ target_link_libraries(controller ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) +install( + DIRECTORY include/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ diff --git a/turtlebot3/global_planner_plugin.xml b/turtlebot3/global_planner_plugin.xml new file mode 100644 index 0000000..9f0ee7d --- /dev/null +++ b/turtlebot3/global_planner_plugin.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + A global planner plugin that creates service request to get a plan and forwards the response to the move_base global planner module. + + + \ No newline at end of file diff --git a/turtlebot3/include/global_planner.h b/turtlebot3/include/global_planner.h new file mode 100644 index 0000000..a323669 --- /dev/null +++ b/turtlebot3/include/global_planner.h @@ -0,0 +1,121 @@ +#ifndef GlobalPlanner_H_ +#define GlobalPlanner_H_ +#include +#include +#include +#include +#include + +#include +#include + +#include +using std::string; + +namespace global_planner{ + /** + * @class GlobalPlanner + * @brief A global planner that creates service request for a plan and forwards the response to the move_base global planner module + */ + class GlobalPlanner : public nav_core::BaseGlobalPlanner { + public: + /** + * @brief Constructor for the GlobalPlanner + */ + GlobalPlanner(); + /** + * @brief Constructor for the GlobalPlanner + * @param name The name of this planner + * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning + */ + GlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Initialization function for the GlobalPlanner + * @param name The name of this planner + * @param costmap_ros A pointer to the ROS wrapper of the costmap to use for planning + */ + void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Given a goal pose in the world, compute a plan + * @param start The start pose + * @param goal The goal pose + * @param plan The plan... filled by the planner + * @return True if a valid plan was found, false otherwise + */ + bool makePlan(const geometry_msgs::PoseStamped& start, + const geometry_msgs::PoseStamped& goal, std::vector& plan); + + /** + * @brief Converts a x,y grid cell coordinate value to a linear index value (one-dimensional array index) + * @param x Grid cell map x coordinate value + * @param y Grid cell map y coordinate value + * @return index value corresponding to the location on the one-dimensional array representation + */ + size_t ToIndex(float x, float y); + + /** + * @brief Converts a linear index value to a x,y grid cell coordinate value + * @param index A linear index value, specifying a cell/pixel in an 1-D array + * @param x Grid cell map x coordinate value + * @param y Grid cell map y coordinate value + */ + void FromIndex(size_t index, int &x, int &y); + + /** + * @brief Converts x,y values in the world frame (in meters) into x,y grid map coordinates + * This transformation is derived from the map resolution and adjusts + * w.r.t the location of the map origin + * @param x X-Axis value in the world frame of reference (in meters) + * @param y Y-Axis value in the world frame of reference (in meters) + */ + void FromWorldToGrid(float &x, float &y); + + /** + * @brief Converts x,y grid cell coordinates to world coordinates (in meters) + * This transformation is derived from the map resolution, adjusts + * w.r.t the location of the map origin and can include an offset + * to place the world coordinate at the center point of a grid cell + * @param x Grid cell map x coordinate value + * @param y Grid cell map y coordinate value + */ + void FromGridToWorld(float &x, float &y); + + /** + * @brief Checks if world coordinates are inside grid map bounds + * @param x X-Axis value in the world frame of reference (in meters) + * @param y Y-Axis value in the world frame of reference (in meters) + * @return true if a index is in map bounds, otherwise false + */ + bool InGridMapBounds(float &x, float &y); + + private: + costmap_2d::Costmap2DROS* costmap_ros_; + costmap_2d::Costmap2D* costmap_; + bool initialized_; + // x,y position (in meters) of grid map origin w.r.t world's coordinate origin + float origin_x_; + float origin_y_; + // the resolution of the map which is expressed in meters per pixel + // or the size of each grid cell (pixel) in meters + // 0.05 means for example 5 centimers for each cell (pixel) + float resolution_; + // by default path is created along the corners/edges of grid cells + bool path_at_node_center = false; + float node_center_offset_ = 0; + // map dimentions in number of grid cells + int width_; + int height_; + int map_size_; + // service client declaration + ros::ServiceClient makeplan_service_; + /** + * @brief Publishes the global plan to display it's entire lenght in RVIZ + * @param path The plan as filled by the planner + */ + void publishPlan(const std::vector &path); + ros::Publisher plan_pub_; + }; +}; +#endif \ No newline at end of file diff --git a/turtlebot3/launch/map_node.launch b/turtlebot3/launch/map_node.launch new file mode 100644 index 0000000..bead355 --- /dev/null +++ b/turtlebot3/launch/map_node.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/turtlebot3/launch/move_base.launch b/turtlebot3/launch/move_base.launch new file mode 100644 index 0000000..ab9a13b --- /dev/null +++ b/turtlebot3/launch/move_base.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/turtlebot3/maps/map.pgm b/turtlebot3/maps/map.pgm new file mode 100644 index 0000000..43312ba Binary files /dev/null and b/turtlebot3/maps/map.pgm differ diff --git a/turtlebot3/maps/map.yaml b/turtlebot3/maps/map.yaml new file mode 100644 index 0000000..590795e --- /dev/null +++ b/turtlebot3/maps/map.yaml @@ -0,0 +1,7 @@ +image: map.pgm +resolution: 0.100000 +origin: [-5.000000, -5.000000, 0.000000] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 + diff --git a/scripts/msg/Points.msg b/turtlebot3/msg/Points.msg similarity index 60% rename from scripts/msg/Points.msg rename to turtlebot3/msg/Points.msg index f644eb9..4b7a2c8 100644 --- a/scripts/msg/Points.msg +++ b/turtlebot3/msg/Points.msg @@ -1,2 +1,2 @@ geometry_msgs/Point[] points -uint8 end_index \ No newline at end of file +float64 end_index \ No newline at end of file diff --git a/scripts/package.xml b/turtlebot3/package.xml similarity index 68% rename from scripts/package.xml rename to turtlebot3/package.xml index 124aad4..cd3168f 100644 --- a/scripts/package.xml +++ b/turtlebot3/package.xml @@ -1,8 +1,8 @@ - scripts + turtlebot3 0.0.0 - The scripts package + The turtlebot3 package @@ -15,11 +15,10 @@ TODO - - + @@ -50,22 +49,44 @@ catkin costmap_2d + actionlib + nav_msgs + tf dynamic_reconfigure message_generation + move_base_msgs roscpp std_msgs + nav_core + pp_msgs + base_local_planner costmap_2d + pp_msgs + nav_msgs + tf + nav_core + base_local_planner + actionlib + move_base_msgs dynamic_reconfigure roscpp costmap_2d + nav_msgs + tf + pp_msgs dynamic_reconfigure roscpp geometry_msgs + actionlib + nav_core + base_local_planner + amcl + map_server + move_base_msgs + turtlebot3_bringup std_msgs message_runtime - - - + diff --git a/turtlebot3/param/global_planner_params.yaml b/turtlebot3/param/global_planner_params.yaml new file mode 100644 index 0000000..02019fe --- /dev/null +++ b/turtlebot3/param/global_planner_params.yaml @@ -0,0 +1,20 @@ + +GlobalPlanner: # Also see: http://wiki.ros.org/global_planner + old_navfn_behavior: false # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false + use_quadratic: true # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true + use_dijkstra: false # Use dijkstra's algorithm. Otherwise, A*, default true + use_grid_path: false # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false + + allow_unknown: true # Allow planner to plan through unknown space, default true + # Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work + planner_window_x: 0.0 # default 0.0 + planner_window_y: 0.0 # default 0.0 + default_tolerance: 0.0 # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0 + + publish_scale: 100 # Scale by which the published potential gets multiplied, default 100 + planner_costmap_publish_frequency: 0.0 # default 0.0 + + lethal_cost: 253 # default 253 + neutral_cost: 50 # default 50 + cost_factor: 3.0 # Factor to multiply each cost from costmap by, default 3.0 + publish_potential: true # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true \ No newline at end of file diff --git a/turtlebot3/src/bfs.cpp b/turtlebot3/src/bfs.cpp new file mode 100755 index 0000000..a1ebc04 --- /dev/null +++ b/turtlebot3/src/bfs.cpp @@ -0,0 +1,384 @@ +// ############### Including headers ################### // + +#include "tf/tf.h" +#include "ros/ros.h" +#include "ros/console.h" +#include "std_msgs/String.h" +#include "nav_msgs/Odometry.h" +#include "std_msgs/Int8MultiArray.h" +#include "pp_msgs/PathPlanningPlugin.h" +#include "pp_msgs/PathPlanningPluginResponse.h" +#include "geometry_msgs/PoseStamped.h" +#include "nav_msgs/OccupancyGrid.h" +#include "geometry_msgs/Point.h" +#include "turtlebot3/Points.h" +#include "bits/stdc++.h" +#include "algorithm" + +// #################################################### // + +// ## Pre-initialzed Variables uses for heights and Lengths of the map ## // + +int rows = 96; +int columns = 96; +int data1[96*96]; + +// ###################################################################### // + + +// ################# Position based Variables ########################### // + +float curr_x, curr_y, curr_theta; +float des_x, des_y, des_theta; + +// ###################################################################### // + + +// ########################## Counters ############################## // + +int odom_sub_count = 0; // Keeps track whether the odometry topic is subscribed or not +bool final_index_found = 0; // Keeps track whether the final index is found or not +int path_pub_count = 0; // Keeps track whether the path has already been created or not +int tar_sub_count = 0 ; // Keeps track whether the final index is available or not + +// ################################################################## // + + +// ################################ Important Data Types ########################################### // + +std::vector> path; // This is going to be the path we traversed + +std::vector, std::pair>> layer; // Will be storing parent and the current node together in the single unvisited layer + +std::stack, std::pair>>> stack_layer; + +int visited[96][96]={-1}; // Marking all nodes as unvisited + +// ################################################################################################## // + + +// ################################ Functions for One time Use ###################################### // + +/* + Function : Current Map + Arguments : Message of type nav_msgs/Occupancy Grid by subscibing to the map topic + Functionality : Get's the Message of the Map topic and stores it in the Map data + Returns : None +*/ +void currentMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) +{ + + rows = msg->info.width; + columns = msg->info.height; + for(int i = 0 ; i < rows*columns ; i++){ + + data1[i] = (msg->data[i]); + } +} + +/* + Function : get position + Arguments : Message of tupe nav_msgs/Odometry by subscribing to the "/odom" Topic + Functionality : Stores the Current position of the bot in the curr_x and curr_y + Returns : None +*/ +void get_position(const nav_msgs::OdometryPtr& msg){ + odom_sub_count++; + curr_x = msg->pose.pose.position.x; + curr_y = msg->pose.pose.position.y; +} + +/* + Function : get 2d position + Arguments : map in the form of 'array - 1D' + Functionality : Converts 1D to a 2D array for better post-processing + Returns : None +*/ +void get_2d_map(int data1[]){ + + for(int i = 0; i < rows ; i++){ + for(int j = 0; j < (columns-1) ; j++){ + visited[i][j] = data1[i*rows+j+1]; + } + } + +} + +/* + Function : distance to pixels + Arguments : 1. Float distance in x + 2. Float distance in y + Functionality : Converts distance in meters to pixel indices according to the resolution of the map + Returns : A pair of integers know as "Indices" +*/ +std::pair distance_to_pixel(float distance_x, float distance_y){ + // 1 pixel = 0.1 meter + std::pair indices; + + indices.first = (distance_x + 5)/0.1 ;// origin of the map is position (-5,-5) + indices.second = (distance_y + 5)/0.1 ;// origin of the map is position (-5,-5) + + return indices; +} + +/* + Function : Pixels to Distance + Arguments : 1. integer index in rows + 2. integer index in columns + Functionality : Converts pixel indices to distance in meters according to the resolution of the map + Returns : A pair of "doubles" know as "Co-ordinates" +*/ +std::pair pixel_to_distance(int index_1, int index_2){ + + std::pair pixels; + + pixels.first = index_1*0.1 - 5; + pixels.second = index_2*0.1 - 5; + + return pixels; +} + +/* + Function : Layer with the help of BFS + Arguments : 1. integer index in rows + 2. integer index in columns + Functionality : Keeps a track of all the unvisited neighbours in the form of layer + Returns : None +*/ +void BFS_stack_builder(int i, int j){ + + visited[i][j] = 1; // Marking current node as visited too + +// East + if(visited[i+1][j] == 0){ + + if( i+1 < rows ){ // Seeing whether the index is even feasible or no + layer.push_back({{i,j},{i+1,j}}); // Parent is going to be its current node + visited[i+1][j] = 1; + } + + } + +// South + if(visited[i][j+1] == 0){ + + if( j+1 < columns ){ + layer.push_back({{i,j}, {i, j+1}}); // Parent is going to be its current node + visited[i][j+1] = 1; + } + + } + +// West + if(visited[i-1][j] == 0 ){ + + if( i-1 >= 0 ){ + layer.push_back({{i,j},{i-1,j}}); + visited[i-1][j] = 1; + } + + } + +// North + if(visited[i][j-1] == 0){ + + if( j-1 >= 0 ){ + layer.push_back({{i,j},{i,j-1}}); + visited[i][j-1] = 1; + } + + } +} + +/* + Function : Tree Building with the help of BFS layer builder + Arguments : 1. integer index in counter + 2. integer index in columns + Functionality : Keeps a track of all the unvisited neighbours in the form of layer + Returns : None +*/ +std::pair build_a_tree( int layer_element_count, int flag, std::pair final_index){ + + std::pair indx; // Current index + std::pair indx_parent; // Current indexs' Parents + + while(!layer.empty()){ + + indx = layer.front().second; + + if(indx == final_index){ + + final_index_found = 1; + indx_parent = layer.front().first; + break; + + } + + layer.erase(layer.begin()); // This will clear the whole layer when the whole for loop is done traversing without deleting the newly added elements of the next layer + + BFS_stack_builder(indx.first, indx.second); // Now building the graph further + + if(layer_element_count == flag){ + + flag = layer.size(); + layer_element_count = 0; + stack_layer.push(layer); + + } + + layer_element_count++; + + } + + return indx_parent; +} + +/* + Function : Build a from stack + Arguments : 1. Pair of integers for the final index + 2. pair of integers for the current index's parent + Functionality : Build a path + Returns : None +*/ +void build_path_from_stack(std::pair final_index, std::pair indx_parent){ + + if(path_pub_count == 0){ // Cheching whether the path has been previously built or not + + path.push_back(final_index); // Pushing the Final Index + stack_layer.pop(); + + std::cout<< "Final index was found\n"; // Just to know whether we have been successful or not + std::vector, std::pair>> templayer; // This layer just stores the top of the stack elements + + while(!stack_layer.empty()){ // Till we reach the initial index + + std::cout<<"indx parent : "<("path_sub", 1000); // Creating a Publishing node for the path + ros::Rate rate(1); // Defining a rate for overall ros operations + int count = 0; + + // Getting the target from the user + std::cout<<"Please Enter the Location in RVIZ and put the orientation here : \n"<>des_x>>des_y; + std::cin>>des_theta; + + tar_sub_count++; + + while (ros::ok()) + { + + get_2d_map(data1); //got the 2d map to operate upon + std::pair initial_index, final_index; // pixels of the current grid cell + turtlebot3::Points P_msg; // This is going to be the final published path + geometry_msgs::Point point_pub; + + if(odom_sub_count > 0 and tar_sub_count > 0){ // Just checking if the subscription is done or not + + initial_index = distance_to_pixel(curr_x, curr_y); + final_index = distance_to_pixel(des_x, des_y); + ROS_INFO("START x %d START y %d, END x %d END y %d\n", initial_index.first, initial_index.second, final_index.first, final_index.second); + visited[initial_index.first][initial_index.second] = 1; // Marking the Current node as visited + + layer.push_back({{-1,-1},{initial_index.first,initial_index.second}}); // Initialzing layer0 with the First node later the elements will be added into it + stack_layer.push(layer); // Now, Stack is a collection of all layers till the final index + + int flag = layer.size()-1; // Flag is the signal that all the possible children in the current array has been added and now u can move on to the next layer + + int layer_element_count = 0; // this will check if all the elements in the layer has been traversed or not + + // ############################################################################################### // + std::pair indx_parent; + indx_parent = build_a_tree(flag, layer_element_count, final_index); // This will give us the parent of the final node + + if(final_index_found){ + + build_path_from_stack(final_index, indx_parent); + + } + else{ + std::cout<<"No path Exists"< path_temp; + path_temp = pixel_to_distance(path[i].first, path[i].second); + + point_pub.x = path_temp.first; + point_pub.y = path_temp.second; + path_pub[i] = point_pub; + } + + P_msg.points.clear(); + P_msg.end_index = des_theta; + int index_z = 0; + // Now declaring the messages with the help of the points + for (auto &it : path_pub) { + geometry_msgs::Point point; + point.x = (it).x; + point.y = (it).y; + point.z = 0; + P_msg.points.push_back(point); + index_z++; + } + for(int j = 0; j> path; // This is going to be the path we traversed + +std::vector, std::pair>> layer; // Will be storing parent and the current node together in the single unvisited layer + +std::stack, std::pair>>> stack_layer; + +int visited[96][96]={-1}; // Marking all nodes as unvisited + +// ################################################################################################## // + + +// ################################ Functions for One time Use ###################################### // + +/* + Function : Current Map + Arguments : Message of type nav_msgs/Occupancy Grid by subscibing to the map topic + Functionality : Get's the Message of the Map topic and stores it in the Map data + Returns : None +*/ +void currentMap(const nav_msgs::OccupancyGrid::ConstPtr &msg) +{ + + rows = msg->info.width; + columns = msg->info.height; + for(int i = 0 ; i < rows*columns ; i++){ + + data1[i] = (msg->data[i]); + } +} + +/* + Function : get position + Arguments : Message of tupe nav_msgs/Odometry by subscribing to the "/odom" Topic + Functionality : Stores the Current position of the bot in the curr_x and curr_y + Returns : None +*/ +void get_position(const nav_msgs::OdometryPtr& msg){ + odom_sub_count++; + curr_x = msg->pose.pose.position.x; + curr_y = msg->pose.pose.position.y; +} + +/* + Function : get 2d position + Arguments : map in the form of 'array - 1D' + Functionality : Converts 1D to a 2D array for better post-processing + Returns : None +*/ +void get_2d_map(int data1[]){ + + for(int i = 0; i < rows ; i++){ + for(int j = 0; j < (columns-1) ; j++){ + visited[i][j] = data1[i*rows+j+1]; + } + } + +} + +/* + Function : distance to pixels + Arguments : 1. Float distance in x + 2. Float distance in y + Functionality : Converts distance in meters to pixel indices according to the resolution of the map + Returns : A pair of integers know as "Indices" +*/ +std::pair distance_to_pixel(float distance_x, float distance_y){ + // 1 pixel = 0.1 meter + std::pair indices; + + indices.first = (distance_x + 5)/0.1 ;// origin of the map is position (-5,-5) + indices.second = (distance_y + 5)/0.1 ;// origin of the map is position (-5,-5) + + return indices; +} + +/* + Function : Pixels to Distance + Arguments : 1. integer index in rows + 2. integer index in columns + Functionality : Converts pixel indices to distance in meters according to the resolution of the map + Returns : A pair of "doubles" know as "Co-ordinates" +*/ +std::pair pixel_to_distance(int index_1, int index_2){ + + std::pair pixels; + + pixels.first = index_1*0.1 - 5; + pixels.second = index_2*0.1 - 5; + + return pixels; +} + +/* + Function : Layer with the help of BFS + Arguments : 1. integer index in rows + 2. integer index in columns + Functionality : Keeps a track of all the unvisited neighbours in the form of layer + Returns : None +*/ +void BFS_stack_builder(int i, int j){ + + visited[i][j] = 1; // Marking current node as visited too + +// East + if(visited[i+1][j] == 0){ + + if( i+1 < rows ){ // Seeing whether the index is even feasible or no + layer.push_back({{i,j},{i+1,j}}); // Parent is go ing to be its current node + visited[i+1][j] = 1; + } + + } + +// South + if(visited[i][j+1] == 0){ + + if( j+1 < columns ){ + layer.push_back({{i,j}, {i, j+1}}); // Parent is going to be its current node + visited[i][j+1] = 1; + } + + } + +// West + if(visited[i-1][j] == 0 ){ + + if( i-1 >= 0 ){ + layer.push_back({{i,j},{i-1,j}}); + visited[i-1][j] = 1; + } + + } + +// North + if(visited[i][j-1] == 0){ + + if( j-1 >= 0 ){ + layer.push_back({{i,j},{i,j-1}}); + visited[i][j-1] = 1; + } + + } +} + +/* + Function : Tree Building with the help of BFS layer builder + Arguments : 1. integer index in counter + 2. integer index in columns + Functionality : Keeps a track of all the unvisited neighbours in the form of layer + Returns : None +*/ +std::pair build_a_tree( int layer_element_count, int flag, std::pair final_index){ + + std::pair indx; // Current index + std::pair indx_parent; // Current indexs' Parents + + while(!layer.empty()){ + + indx = layer.front().second; + + if(indx == final_index){ + + final_index_found = 1; + indx_parent = layer.front().first; + break; + + } + + layer.erase(layer.begin()); // This will clear the whole layer when the whole for loop is done traversing without deleting the newly added elements of the next layer + + BFS_stack_builder(indx.first, indx.second); // Now building the graph further + + if(layer_element_count == flag){ + + flag = layer.size(); + layer_element_count = 0; + stack_layer.push(layer); + + } + + layer_element_count++; + + } + + return indx_parent; +} + +/* + Function : Build a from stack + Arguments : 1. Pair of integers for the final index + 2. pair of integers for the current index's parent + Functionality : Build a path + Returns : None +*/ +void build_path_from_stack(std::pair final_index, std::pair indx_parent){ + + if(path_pub_count >= 0){ // Cheching whether the path has been previously built or not + + path.push_back(final_index); // Pushing the Final Index + stack_layer.pop(); + path.clear(); + std::cout<< "Final index was found\n"; // Just to know whether we have been successful or not + std::vector, std::pair>> templayer; // This layer just stores the top of the stack elements + + while(!stack_layer.empty()){ // Till we reach the initial index + + std::cout<<"indx parent : "< costmap){ + + ROS_INFO("+-----------------+ +++++++ +-------------------+\n"); + ROS_INFO("+-----------------+ Running +-------------------+\n"); + ROS_INFO("+-----------------+ +++++++ +-------------------+\n"); + + std::pair initial_index, final_index; // pixels of the current grid cell + + + final_index.first = ( end_index / width ); // Need to verify this + final_index.second = ( end_index % height ); + + initial_index.first = ( start_index / width ); + initial_index.second = ( start_index % width ); + + ROS_INFO("end : %d , end : %d\n", final_index.first, final_index.second); + ROS_INFO("start : %d , start : %d\n", initial_index.first, initial_index.second); + + get_2d_map(data1); //got the 2d map to operate upon + + turtlebot3::Points P_msg; // This is going to be the final published path + geometry_msgs::Point point_pub; + + if(odom_sub_count > 0){ // Just checking if the subscription is done or not + + visited[initial_index.first][initial_index.second] = 1; // Marking the Current node as visited + + layer.push_back({{-1,-1},{initial_index.first,initial_index.second}}); // Initialzing layer0 with the First node later the elements will be added into it + stack_layer.push(layer); // Now, Stack is a collection of all layers till the final index + + int flag = layer.size()-1; // Flag is the signal that all the possible children in the current array has been added and now u can move on to the next layer + + int layer_element_count = 0; // this will check if all the elements in the layer has been traversed or not + + // ############################################################################################### // + std::pair indx_parent; + indx_parent = build_a_tree(flag, layer_element_count, final_index); // This will give us the parent of the final node + + if(final_index_found){ + + build_path_from_stack(final_index, indx_parent); + + } + else{ + return P_msg; + } + // Now as there is some path existing + + } + + // ############### Converting the path in terms of the message that we are going to publish ############# // + int x_t = path.size(); + + geometry_msgs::Point path_pub[x_t]; + + for(int i = 0; i < path.size() ; i++){ + std::pair path_temp; + path_temp = pixel_to_distance(path[i].first, path[i].second); + + point_pub.x = path_temp.first; + point_pub.y = path_temp.second; + path_pub[i] = point_pub; + } + + P_msg.points.clear(); + P_msg.end_index = des_theta; + int index_z = 0; + // Now declaring the messages with the help of the points + for (auto &it : path_pub) { + geometry_msgs::Point point; + point.x = (it).x; + point.y = (it).y; + point.z = 0; + P_msg.points.push_back(point); + index_z++; + } + return P_msg; +} + +/* + Function : makePlan - A standard node for different algorithms + Arguments : Service Request, Service Response + Funcitonality : Publish a path for move_base + Returns : bool that describes whether path is beign published or not +*/ +bool make_plan(pp_msgs::PathPlanningPlugin::Request &req, pp_msgs::PathPlanningPlugin::Response &res){ + std::vector costmap; + std::vector path_bfs; + costmap = req.costmap_ros; + + int width = req.width; + int height = req.height; + + int start_index = req.start; + int end_index = req.goal; + float resolution = 0.1; + + turtlebot3::Points path_from_bfs = bfs(start_index, end_index, width, height, resolution, costmap); + + if(path_from_bfs.points.size() == 0){ + ROS_INFO("!!! NO PATH FOUND !!!\n"); + return false; + } + else{ + ROS_INFO("!!! PATH FOUND !!!\n"); + } + + path_bfs.clear(); + for (auto &itx : path_from_bfs.points ){ + + std::pair idx_2d = distance_to_pixel(itx.x, itx.y); + ROS_INFO(" x : %d y : %d \n", idx_2d.first, idx_2d.second); + int idx_1d = idx_2d.first * width + idx_2d.second; + path_bfs.push_back(idx_1d); + + std::reverse(path_bfs.begin(), path_bfs.end()); + } + + res.plan = path_bfs; + + return true; +} + +// ##################################################################################################### // +// ##################################### Main Function ########################################## // +// ##################################################################################################### // + +int main(int argc, char **argv) { + + // ########################## Creating Ros nodes, Subsciber, and Publishers ########################## // + ros::init(argc, argv, "bfs_server"); + ros::NodeHandle n; // Creating a nodehandler to make relevant subscribers and publishers + + ros::Subscriber subscriber = n.subscribe("map",1000, currentMap); // Subscribing to map topic + ros::Subscriber odo_sub = n.subscribe("odom", 1000, get_position); // Subscribing to odom topic + + ros::ServiceServer service = n.advertiseService("/move_base/GlobalPlanner/make_plan", make_plan); + + ROS_INFO("Ready to publish the path\n"); + + ros::spin(); + return 0; +} + +// ###################################################################################################### // +// ############################################# The End ############################################## // +// ###################################################################################################### // \ No newline at end of file diff --git a/turtlebot3/src/global_planner.cpp b/turtlebot3/src/global_planner.cpp new file mode 100644 index 0000000..f5ba3e2 --- /dev/null +++ b/turtlebot3/src/global_planner.cpp @@ -0,0 +1,205 @@ +#include +#include +#include +#include + +// register this planner as a BaseGlobalPlanner plugin +PLUGINLIB_EXPORT_CLASS(global_planner::GlobalPlanner, nav_core::BaseGlobalPlanner) + + +namespace global_planner +{ + + GlobalPlanner::GlobalPlanner() + { + initialized_ = false; + } + + GlobalPlanner::GlobalPlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros) + { + initialized_ = false; + initialize(name, costmap_ros); + } + + void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros) + { + if (!initialized_) + { + ros::NodeHandle private_nh("~/" + name); + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros->getCostmap(); + origin_x_ = costmap_->getOriginX(); + origin_y_ = costmap_->getOriginY(); + + width_ = costmap_->getSizeInCellsX(); + height_ = costmap_->getSizeInCellsY(); + resolution_ = costmap_->getResolution(); + map_size_ = width_ * height_; + + // create a client for the path planning service + makeplan_service_ = private_nh.serviceClient("/move_base/GlobalPlanner/make_plan"); + // wait for the service to be advertised and available, blocks until it is. + makeplan_service_.waitForExistence(); + // create publisher to display the complete trajectory path in RVIZ + plan_pub_ = private_nh.advertise("plan", 1); + + // place path waypoints at the center of each grid cell (vs. at the corners of grid cells) + path_at_node_center = true; + if (path_at_node_center) + { + // shift all of the coordinates by half a grid cell + node_center_offset_ = resolution_ / 2; + } + + initialized_ = true; + } + } + + // fill plan request, call plan service, process plan response + bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector &plan) + { + plan.clear(); + + // Fill costmap (costmap is a 1-D array map representation) + std::vector costmap(map_size_); + + for (size_t idx = 0; idx < map_size_; ++idx) + { + int x, y; + x = idx % width_; + y = std::floor(idx / width_); + costmap.at(idx) = static_cast(costmap_->getCost(x, y)); + } + + float start_x = start.pose.position.x; + float start_y = start.pose.position.y; + float goal_x = goal.pose.position.x; + float goal_y = goal.pose.position.y; + + size_t start_index = 0; + size_t goal_index = 0; + + // check if start/goal world coordinates are inside grid map bounds + if (InGridMapBounds(start_x, start_y) && InGridMapBounds(goal_x, goal_y)) + { + // convert x,y in world coordinates/meters) to x,y in grid map cell coordinates + FromWorldToGrid(start_x, start_y); + FromWorldToGrid(goal_x, goal_y); + + // convert 2d representation into flat array index representation + start_index = ToIndex(start_x, start_y); + goal_index = ToIndex(goal_x, goal_y); + } + else + { + ROS_WARN("Start or goal position outside of the map's boundaries"); + return false; + } + + // To-Do: check that a start and goal are not obstacles + pp_msgs::PathPlanningPlugin makeplan; + makeplan.request.costmap_ros = costmap; + makeplan.request.start = start_index; + makeplan.request.goal = goal_index; + makeplan.request.width = width_; + makeplan.request.height = height_; + + // call path planning service + makeplan_service_.call(makeplan); + + std::vector index_plan = makeplan.response.plan; + + ROS_DEBUG("Number of points: %d", unsigned(index_plan.size())); + + /* Process plan response */ + if (index_plan.size()) + { + // insert start node into plan response + index_plan.insert(index_plan.begin(), start_index); + // insert goal node into plan response + index_plan.push_back(goal_index); + + for (int p : index_plan) + { + int x, y; + FromIndex(p, x, y); + float x_path = static_cast(x); + float y_path = static_cast(y); + + FromGridToWorld(x_path, y_path); + geometry_msgs::PoseStamped position; + position.header.frame_id = start.header.frame_id; + position.pose.position.x = x_path; + position.pose.position.y = y_path; + position.pose.orientation.x = 0; + position.pose.orientation.y = 0; + position.pose.orientation.z = 0; + position.pose.orientation.w = 1; + + plan.push_back(position); + } + + plan.push_back(goal); + + // Publish the path for visualisation + publishPlan(plan); + + return true; + } + else + { + // no plan found + return false; + } + } + + void GlobalPlanner::publishPlan(const std::vector &path) + { + + // create a message + nav_msgs::Path gui_path; + gui_path.poses.resize(path.size()); + + gui_path.header.frame_id = "map"; + gui_path.header.stamp = ros::Time::now(); + + // Extract the plan in world coordinates + for (unsigned int i = 0; i < path.size(); i++) + { + gui_path.poses[i] = path[i]; + } + + plan_pub_.publish(gui_path); + } + + size_t GlobalPlanner::ToIndex(float x, float y) + { + return y * width_ + x; + } + + void GlobalPlanner::FromIndex(size_t index, int &x, int &y) + { + x = index % width_; + y = std::floor(index / width_); + } + + void GlobalPlanner::FromWorldToGrid(float &x, float &y) + { + x = static_cast((x - origin_x_) / resolution_); + y = static_cast((y - origin_y_) / resolution_); + } + + void GlobalPlanner::FromGridToWorld(float &x, float &y) + { + x = x * resolution_ + origin_x_ + node_center_offset_; + y = y * resolution_ + origin_y_ + node_center_offset_; + } + + bool GlobalPlanner::InGridMapBounds(float &x, float &y) + { + if (x < origin_x_ || y < origin_y_ || x > origin_x_ + (width_ * resolution_) || y > origin_y_ + (height_ * resolution_)) + return false; + return true; + } + +}; // namespace global_planner \ No newline at end of file diff --git a/turtlebot3_fake/CHANGELOG.rst b/turtlebot3_fake/CHANGELOG.rst deleted file mode 100644 index b1b36e2..0000000 --- a/turtlebot3_fake/CHANGELOG.rst +++ /dev/null @@ -1,99 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package turtlebot3_fake -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.3.2 (2021-07-13) ------------------- -* update catkin minimum requirement -* fix bugs -* Contributors: Kerui, Elvis Dowson, Will Son - -1.3.1 (2021-01-07) ------------------- -* fix init() in turtlebot3_drive.cpp -* Apply low poly models -* Noetic release -* Contributors: Sean Yen, Will Son - -1.3.0 (2020-09-29) ------------------- -* fix ROS Assert issue when debugging (#124) -* added TurtleBot3 Autorace 2020 (#108) -* added turtlebot3_description to dependency list (#104) -* removed *nix path separator (#92) -* removed unnecessary gazebo plugin_path (#78) -* Contributors: Sean Yen, Ashe Kim, Mikael Arguedas, Ben Wolsieffer - -1.2.0 (2019-01-22) ------------------- -* move out the init() from ROS_ASSERT `#68 `_ -* Contributors: Sean Yen, Darby Lim, Pyo - -1.1.0 (2018-07-20) ------------------- -* added TurtleBot3 Waffle Pi -* Contributors: Darby Lim, Pyo - -1.0.2 (2018-06-01) ------------------- -* none - -1.0.1 (2018-05-30) ------------------- -* none - -1.0.0 (2018-05-29) ------------------- -* none - -0.2.4 (2018-03-14) ------------------- -* none - -0.2.3 (2018-03-14) ------------------- -* none - -0.2.2 (2018-03-14) ------------------- -* none - -0.2.1 (2018-03-14) ------------------- -* none - -0.2.0 (2018-03-13) ------------------- -* added TurtleBot3 Waffle Pi -* Contributors: Darby Lim - -0.1.7 (2017-08-16) ------------------- -* none - -0.1.6 (2017-08-14) ------------------- -* updated rviz and add static tf publisher for depth camera -* Contributors: Darby Lim - -0.1.5 (2017-06-09) ------------------- -* none - -0.1.4 (2017-05-23) ------------------- -* added as new meta-packages and version update (0.1.4) -* Contributors: Darby Lim, Pyo - -0.1.3 (2017-04-24) ------------------- -* modified the package information for release -* modified SLAM param -* modified the description, authors, depend option and delete the core package -* modified the turtlebot bringup files -* modified pkg setting for turtlebot3_core -* modified the navigation package and turtlebot3 node for demo -* modified the wheel speed gain -* added Intel RealSense R200 -* added LDS sensor -* Contributors: Darby Lim, Pyo diff --git a/turtlebot3_fake/CMakeLists.txt b/turtlebot3_fake/CMakeLists.txt deleted file mode 100644 index afb3344..0000000 --- a/turtlebot3_fake/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 3.0.2) -project(turtlebot3_fake) -cmake_policy(SET CMP0054 NEW) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - tf - turtlebot3_msgs -) - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp std_msgs sensor_msgs geometry_msgs nav_msgs tf turtlebot3_msgs -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_executable(turtlebot3_fake_node src/turtlebot3_fake.cpp) -add_dependencies(turtlebot3_fake_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(turtlebot3_fake_node ${catkin_LIBRARIES}) - -################################################################################ -# Install -################################################################################ -install(TARGETS turtlebot3_fake_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ diff --git a/turtlebot3_fake/include/turtlebot3_fake/turtlebot3_fake.h b/turtlebot3_fake/include/turtlebot3_fake/turtlebot3_fake.h deleted file mode 100644 index d34b9fe..0000000 --- a/turtlebot3_fake/include/turtlebot3_fake/turtlebot3_fake.h +++ /dev/null @@ -1,114 +0,0 @@ -/******************************************************************************* -* Copyright 2016 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Yoonseok Pyo */ - -#ifndef TURTLEBOT3_FAKE_H_ -#define TURTLEBOT3_FAKE_H_ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "turtlebot3_fake.h" - -#define WHEEL_RADIUS 0.033 // meter - -#define LEFT 0 -#define RIGHT 1 - -#define MAX_LINEAR_VELOCITY 0.22 // m/s -#define MAX_ANGULAR_VELOCITY 2.84 // rad/s -#define VELOCITY_STEP 0.01 // m/s -#define VELOCITY_LINEAR_X 0.01 // m/s -#define VELOCITY_ANGULAR_Z 0.1 // rad/s -#define SCALE_VELOCITY_LINEAR_X 1 -#define SCALE_VELOCITY_ANGULAR_Z 1 - -#define DEG2RAD(x) (x * 0.01745329252) // *PI/180 -#define RAD2DEG(x) (x * 57.2957795131) // *180/PI - -#define TORQUE_ENABLE 1 // Value for enabling the torque of motor -#define TORQUE_DISABLE 0 // Value for disabling the torque of motor - -class Turtlebot3Fake -{ - public: - Turtlebot3Fake(); - ~Turtlebot3Fake(); - bool init(); - bool update(); - - private: - // ROS NodeHandle - ros::NodeHandle nh_; - ros::NodeHandle nh_priv_; - - // ROS Parameters - // (TODO) - - // ROS Time - ros::Time last_cmd_vel_time_; - ros::Time prev_update_time_; - - // ROS Topic Publishers - ros::Publisher joint_states_pub_; - ros::Publisher odom_pub_; - - // ROS Topic Subscribers - ros::Subscriber cmd_vel_sub_; - - sensor_msgs::JointState joint_states_; - nav_msgs::Odometry odom_; - tf::TransformBroadcaster tf_broadcaster_; - - double wheel_speed_cmd_[2]; - double goal_linear_velocity_; - double goal_angular_velocity_; - double cmd_vel_timeout_; - - float odom_pose_[3]; - float odom_vel_[3]; - double pose_cov_[36]; - - std::string joint_states_name_[2]; - - double last_position_[2]; - double last_velocity_[2]; - - double wheel_seperation_; - double turning_radius_; - double robot_radius_; - - // Function prototypes - void commandVelocityCallback(const geometry_msgs::TwistConstPtr cmd_vel_msg); - bool updateOdometry(ros::Duration diff_time); - void updateJoint(void); - void updateTF(geometry_msgs::TransformStamped& odom_tf); -}; - -#endif // TURTLEBOT3_FAKE_H_ diff --git a/turtlebot3_fake/launch/turtlebot3_fake.launch b/turtlebot3_fake/launch/turtlebot3_fake.launch deleted file mode 100644 index 4b66f34..0000000 --- a/turtlebot3_fake/launch/turtlebot3_fake.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/turtlebot3_fake/package.xml b/turtlebot3_fake/package.xml deleted file mode 100644 index a2ec825..0000000 --- a/turtlebot3_fake/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - turtlebot3_fake - 1.3.2 - - Package for TurtleBot3 fake node. With this package, simple tests can be done without a robot. - You can do simple tests using this package on rviz without real robots. - - Apache 2.0 - Pyo - Darby Lim - Will Son - http://wiki.ros.org/turtlebot3_fake - http://turtlebot3.robotis.com - https://github.com/ROBOTIS-GIT/turtlebot3_simulations - https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues - catkin - roscpp - std_msgs - sensor_msgs - geometry_msgs - nav_msgs - tf - turtlebot3_msgs - robot_state_publisher - diff --git a/turtlebot3_fake/rviz/turtlebot3_fake.rviz b/turtlebot3_fake/rviz/turtlebot3_fake.rviz deleted file mode 100644 index 0b8c7a3..0000000 --- a/turtlebot3_fake/rviz/turtlebot3_fake.rviz +++ /dev/null @@ -1,242 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /RobotModel1 - - /Odometry1 - Splitter Ratio: 0.6029411554336548 - Tree Height: 745 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: - Value: true - - Class: rviz/Axes - Enabled: true - Length: 0.10000000149011612 - Name: Axes - Radius: 0.009999999776482582 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_scan: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_depth_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_depth_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_rgb_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_rgb_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - caster_back_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - caster_back_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wheel_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: false - Enabled: true - Keep: 100 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: /odom - Unreliable: false - Value: true - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 0.5 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - {} - Update Interval: 0 - Value: false - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Fixed Frame: odom - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/XYOrbit - Distance: 3.500234365463257 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.021406516432762146 - Y: 0.01958480477333069 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.33039841055870056 - Target Frame: - Value: XYOrbit (rviz) - Yaw: 1.0253981351852417 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1027 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000373fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003500000373000000b600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000373fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000035000003730000009900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009c70000003efc0100000002fb0000000800540069006d00650100000000000009c70000024b00fffffffb0000000800540069006d006501000000000000045000000000000000000000075a0000037300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 2503 - X: 55 - Y: 0 diff --git a/turtlebot3_fake/src/turtlebot3_fake.cpp b/turtlebot3_fake/src/turtlebot3_fake.cpp deleted file mode 100644 index 431c680..0000000 --- a/turtlebot3_fake/src/turtlebot3_fake.cpp +++ /dev/null @@ -1,258 +0,0 @@ -/******************************************************************************* -* Copyright 2016 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Yoonseok Pyo */ - -#include - -Turtlebot3Fake::Turtlebot3Fake() -: nh_priv_("~") -{ - //Init fake turtlebot node - bool init_result = init(); - ROS_ASSERT(init_result); -} - -Turtlebot3Fake::~Turtlebot3Fake() -{ -} - -/******************************************************************************* -* Init function -*******************************************************************************/ -bool Turtlebot3Fake::init() -{ - // initialize ROS parameter - - std::string robot_model = nh_.param("tb3_model", ""); - - if (!robot_model.compare("burger")) - { - wheel_seperation_ = 0.160; - turning_radius_ = 0.080; - robot_radius_ = 0.105; - } - else if (!robot_model.compare("waffle") || !robot_model.compare("waffle_pi")) - { - wheel_seperation_ = 0.287; - turning_radius_ = 0.1435; - robot_radius_ = 0.220; - } - - nh_.param("wheel_left_joint_name", joint_states_name_[LEFT], std::string("wheel_left_joint")); - nh_.param("wheel_right_joint_name", joint_states_name_[RIGHT], std::string("wheel_right_joint")); - nh_.param("joint_states_frame", joint_states_.header.frame_id, std::string("base_footprint")); - nh_.param("odom_frame", odom_.header.frame_id, std::string("odom")); - nh_.param("base_frame", odom_.child_frame_id, std::string("base_footprint")); - - // initialize variables - wheel_speed_cmd_[LEFT] = 0.0; - wheel_speed_cmd_[RIGHT] = 0.0; - goal_linear_velocity_ = 0.0; - goal_angular_velocity_ = 0.0; - cmd_vel_timeout_ = 1.0; - last_position_[LEFT] = 0.0; - last_position_[RIGHT] = 0.0; - last_velocity_[LEFT] = 0.0; - last_velocity_[RIGHT] = 0.0; - - double pcov[36] = { 0.1, 0, 0, 0, 0, 0, - 0, 0.1, 0, 0, 0, 0, - 0, 0, 1e6, 0, 0, 0, - 0, 0, 0, 1e6, 0, 0, - 0, 0, 0, 0, 1e6, 0, - 0, 0, 0, 0, 0, 0.2}; - memcpy(&(odom_.pose.covariance),pcov,sizeof(double)*36); - memcpy(&(odom_.twist.covariance),pcov,sizeof(double)*36); - - odom_pose_[0] = 0.0; - odom_pose_[1] = 0.0; - odom_pose_[2] = 0.0; - - odom_vel_[0] = 0.0; - odom_vel_[1] = 0.0; - odom_vel_[2] = 0.0; - - joint_states_.name.push_back(joint_states_name_[LEFT]); - joint_states_.name.push_back(joint_states_name_[RIGHT]); - joint_states_.position.resize(2,0.0); - joint_states_.velocity.resize(2,0.0); - joint_states_.effort.resize(2,0.0); - - // initialize publishers - joint_states_pub_ = nh_.advertise("joint_states", 100); - odom_pub_ = nh_.advertise("odom", 100); - - // initialize subscribers - cmd_vel_sub_ = nh_.subscribe("cmd_vel", 100, &Turtlebot3Fake::commandVelocityCallback, this); - - prev_update_time_ = ros::Time::now(); - - return true; -} - -/******************************************************************************* -* Callback function for cmd_vel msg -*******************************************************************************/ -void Turtlebot3Fake::commandVelocityCallback(const geometry_msgs::TwistConstPtr cmd_vel_msg) -{ - last_cmd_vel_time_ = ros::Time::now(); - - goal_linear_velocity_ = cmd_vel_msg->linear.x; - goal_angular_velocity_ = cmd_vel_msg->angular.z; - - wheel_speed_cmd_[LEFT] = goal_linear_velocity_ - (goal_angular_velocity_ * wheel_seperation_ / 2); - wheel_speed_cmd_[RIGHT] = goal_linear_velocity_ + (goal_angular_velocity_ * wheel_seperation_ / 2); -} - -/******************************************************************************* -* Calculate the odometry -*******************************************************************************/ -bool Turtlebot3Fake::updateOdometry(ros::Duration diff_time) -{ - double wheel_l, wheel_r; // rotation value of wheel [rad] - double delta_s, delta_theta; - double v[2], w[2]; - - wheel_l = wheel_r = 0.0; - delta_s = delta_theta = 0.0; - - v[LEFT] = wheel_speed_cmd_[LEFT]; - w[LEFT] = v[LEFT] / WHEEL_RADIUS; // w = v / r - v[RIGHT] = wheel_speed_cmd_[RIGHT]; - w[RIGHT] = v[RIGHT] / WHEEL_RADIUS; - - last_velocity_[LEFT] = w[LEFT]; - last_velocity_[RIGHT] = w[RIGHT]; - - wheel_l = w[LEFT] * diff_time.toSec(); - wheel_r = w[RIGHT] * diff_time.toSec(); - - if(isnan(wheel_l)) - { - wheel_l = 0.0; - } - - if(isnan(wheel_r)) - { - wheel_r = 0.0; - } - - last_position_[LEFT] += wheel_l; - last_position_[RIGHT] += wheel_r; - - delta_s = WHEEL_RADIUS * (wheel_r + wheel_l) / 2.0; - delta_theta = WHEEL_RADIUS * (wheel_r - wheel_l) / wheel_seperation_; - - // compute odometric pose - odom_pose_[0] += delta_s * cos(odom_pose_[2] + (delta_theta / 2.0)); - odom_pose_[1] += delta_s * sin(odom_pose_[2] + (delta_theta / 2.0)); - odom_pose_[2] += delta_theta; - - // compute odometric instantaneouse velocity - odom_vel_[0] = delta_s / diff_time.toSec(); // v - odom_vel_[1] = 0.0; - odom_vel_[2] = delta_theta / diff_time.toSec(); // w - - odom_.pose.pose.position.x = odom_pose_[0]; - odom_.pose.pose.position.y = odom_pose_[1]; - odom_.pose.pose.position.z = 0; - odom_.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odom_pose_[2]); - - // We should update the twist of the odometry - odom_.twist.twist.linear.x = odom_vel_[0]; - odom_.twist.twist.angular.z = odom_vel_[2]; - - return true; -} - -/******************************************************************************* -* Calculate the joint states -*******************************************************************************/ -void Turtlebot3Fake::updateJoint(void) -{ - joint_states_.position[LEFT] = last_position_[LEFT]; - joint_states_.position[RIGHT] = last_position_[RIGHT]; - joint_states_.velocity[LEFT] = last_velocity_[LEFT]; - joint_states_.velocity[RIGHT] = last_velocity_[RIGHT]; -} - -/******************************************************************************* -* Calculate the TF -*******************************************************************************/ -void Turtlebot3Fake::updateTF(geometry_msgs::TransformStamped& odom_tf) -{ - odom_tf.header = odom_.header; - odom_tf.child_frame_id = odom_.child_frame_id; - odom_tf.transform.translation.x = odom_.pose.pose.position.x; - odom_tf.transform.translation.y = odom_.pose.pose.position.y; - odom_tf.transform.translation.z = odom_.pose.pose.position.z; - odom_tf.transform.rotation = odom_.pose.pose.orientation; -} - -/******************************************************************************* -* Update function -*******************************************************************************/ -bool Turtlebot3Fake::update() -{ - ros::Time time_now = ros::Time::now(); - ros::Duration step_time = time_now - prev_update_time_; - prev_update_time_ = time_now; - - // zero-ing after timeout - if((time_now - last_cmd_vel_time_).toSec() > cmd_vel_timeout_) - { - wheel_speed_cmd_[LEFT] = 0.0; - wheel_speed_cmd_[RIGHT] = 0.0; - } - - // odom - updateOdometry(step_time); - odom_.header.stamp = time_now; - odom_pub_.publish(odom_); - - // joint_states - updateJoint(); - joint_states_.header.stamp = time_now; - joint_states_pub_.publish(joint_states_); - - // tf - geometry_msgs::TransformStamped odom_tf; - updateTF(odom_tf); - tf_broadcaster_.sendTransform(odom_tf); - - return true; -} - -/******************************************************************************* -* Main function -*******************************************************************************/ -int main(int argc, char* argv[]) -{ - ros::init(argc, argv, "turtlebot3_fake_node"); - Turtlebot3Fake tb3fake; - - ros::Rate loop_rate(30); - - while (ros::ok()) - { - tb3fake.update(); - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} diff --git a/turtlebot3_gazebo/src/turtlebot3_drive.cpp b/turtlebot3_gazebo/src/turtlebot3_drive.cpp old mode 100644 new mode 100755 index b45de54..436bd21 --- a/turtlebot3_gazebo/src/turtlebot3_drive.cpp +++ b/turtlebot3_gazebo/src/turtlebot3_drive.cpp @@ -18,6 +18,7 @@ #include "turtlebot3_gazebo/turtlebot3_drive.h" + Turtlebot3Drive::Turtlebot3Drive() : nh_priv_("~") { @@ -163,6 +164,7 @@ int main(int argc, char* argv[]) { ros::init(argc, argv, "turtlebot3_drive"); Turtlebot3Drive turtlebot3_drive; + ros::NodeHandle n; ros::Rate loop_rate(125); diff --git a/turtlebot3_simulations b/turtlebot3_simulations index e9d809c..64742b6 160000 --- a/turtlebot3_simulations +++ b/turtlebot3_simulations @@ -1 +1 @@ -Subproject commit e9d809ca8e3bf889c0275e4103b15a341ffab888 +Subproject commit 64742b606618b525a2e739cc149f559127b9d373