├── msg └── PointArray.msg ├── maps ├── r0_0.map.pgm ├── r0_1.map.pgm ├── r1_0.map.pgm ├── r1_1.map.pgm ├── rs_0.map.pgm ├── rs_1.map.pgm ├── r0_0.map.yaml ├── r0_1.map.yaml ├── r1_0.map.yaml ├── r1_1.map.yaml ├── rs_0.map.yaml └── rs_1.map.yaml ├── scripts ├── functions.pyc ├── functions_real_two_robots.pyc ├── getfrontier.py ├── recordDistance_two_robots.py ├── recordDistance.py ├── recordDistance_two_robots_real.py ├── frontier_opencv_detector.py ├── assigner.py ├── assigner_real_two_robots.py ├── filter.py ├── filter_real_two_robots.py ├── filter_backup.py ├── newassigner.py ├── functions.py └── functions_real_two_robots.py ├── LICENSE.md ├── package.xml ├── CMakeLists.txt ├── include ├── functions.h └── mtrand.h ├── launch ├── simple.launch ├── single.launch ├── three_tb3.launch ├── three_robots.launch ├── rrt_three_robots.launch ├── two_robots.launch └── two_robots_real.launch ├── src ├── mtrand.cpp ├── functions.cpp ├── global_rrt_detector.cpp ├── global_rrt_detector_real_two_robots.cpp ├── local_rrt_detector.cpp └── local_rrt_detector_real_two_robots.cpp └── readme.md /msg/PointArray.msg: -------------------------------------------------------------------------------- 1 | # An array of points 2 | 3 | geometry_msgs/Point[] points 4 | -------------------------------------------------------------------------------- /maps/r0_0.map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GradyM2M/multi_rrt_exploration/HEAD/maps/r0_0.map.pgm -------------------------------------------------------------------------------- /maps/r0_1.map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GradyM2M/multi_rrt_exploration/HEAD/maps/r0_1.map.pgm -------------------------------------------------------------------------------- /maps/r1_0.map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GradyM2M/multi_rrt_exploration/HEAD/maps/r1_0.map.pgm -------------------------------------------------------------------------------- /maps/r1_1.map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GradyM2M/multi_rrt_exploration/HEAD/maps/r1_1.map.pgm -------------------------------------------------------------------------------- /maps/rs_0.map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GradyM2M/multi_rrt_exploration/HEAD/maps/rs_0.map.pgm -------------------------------------------------------------------------------- /maps/rs_1.map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GradyM2M/multi_rrt_exploration/HEAD/maps/rs_1.map.pgm -------------------------------------------------------------------------------- /scripts/functions.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GradyM2M/multi_rrt_exploration/HEAD/scripts/functions.pyc -------------------------------------------------------------------------------- /scripts/functions_real_two_robots.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GradyM2M/multi_rrt_exploration/HEAD/scripts/functions_real_two_robots.pyc -------------------------------------------------------------------------------- /maps/r0_0.map.yaml: -------------------------------------------------------------------------------- 1 | image: r0_0.map.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/r0_1.map.yaml: -------------------------------------------------------------------------------- 1 | image: r0_1.map.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/r1_0.map.yaml: -------------------------------------------------------------------------------- 1 | image: r1_0.map.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -21.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/r1_1.map.yaml: -------------------------------------------------------------------------------- 1 | image: r1_1.map.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/rs_0.map.yaml: -------------------------------------------------------------------------------- 1 | image: rs_0.map.pgm 2 | resolution: 0.050000 3 | origin: [-12.000000, -18.800000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/rs_1.map.yaml: -------------------------------------------------------------------------------- 1 | image: rs_1.map.pgm 2 | resolution: 0.050000 3 | origin: [-12.000000, -8.400000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 Hassan Umari 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multi_rrt_exploration 4 | 1.0.0 5 | A ROS package that implements a multi-robot RRT-based map exploration algorithm. It also has the image-based frontier detection that uses image processing to extract frontier points 6 | 7 | Hassan Umari 8 | Hassan Umari 9 | 10 | MIT 11 | http://wiki.ros.org/rrt_exploration 12 | 13 | 14 | catkin 15 | geometry_msgs 16 | nav_msgs 17 | roscpp 18 | rospy 19 | std_msgs 20 | tf 21 | visualization_msgs 22 | message_generation 23 | geometry_msgs 24 | nav_msgs 25 | roscpp 26 | rospy 27 | std_msgs 28 | tf 29 | visualization_msgs 30 | message_runtime 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(multi_rrt_exploration) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | nav_msgs 10 | roscpp 11 | rospy 12 | std_msgs 13 | tf 14 | visualization_msgs 15 | message_generation 16 | ) 17 | 18 | 19 | add_message_files( 20 | FILES 21 | PointArray.msg 22 | ) 23 | 24 | generate_messages( 25 | DEPENDENCIES 26 | std_msgs 27 | geometry_msgs 28 | ) 29 | 30 | 31 | catkin_package( 32 | 33 | CATKIN_DEPENDS message_runtime 34 | ) 35 | 36 | 37 | ########### 38 | ## Build ## 39 | ########### 40 | # c++11 support required 41 | include(CheckCXXCompilerFlag) 42 | check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11) 43 | if(COMPILER_SUPPORTS_CXX11) 44 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 45 | else() 46 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 47 | endif() 48 | 49 | 50 | include_directories(include ${catkin_INCLUDE_DIRS}) 51 | 52 | add_executable(global_rrt_detector src/global_rrt_detector.cpp src/functions.cpp src/mtrand.cpp) 53 | target_link_libraries(global_rrt_detector ${catkin_LIBRARIES}) 54 | 55 | add_executable(local_rrt_detector src/local_rrt_detector.cpp src/functions.cpp src/mtrand.cpp) 56 | target_link_libraries(local_rrt_detector ${catkin_LIBRARIES}) 57 | -------------------------------------------------------------------------------- /include/functions.h: -------------------------------------------------------------------------------- 1 | #ifndef functions_H 2 | #define functions_H 3 | #include "ros/ros.h" 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "nav_msgs/OccupancyGrid.h" 10 | #include "geometry_msgs/Point.h" 11 | #include "geometry_msgs/PointStamped.h" 12 | #include "visualization_msgs/Marker.h" 13 | 14 | // rdm class, for gentaring random flot numbers 15 | class rdm{ 16 | int i; 17 | public: 18 | rdm(); 19 | float randomize(); 20 | }; 21 | 22 | 23 | //Norm function prototype 24 | float Norm( std::vector , std::vector ); 25 | 26 | //sign function prototype 27 | float sign(float ); 28 | 29 | //Nearest function prototype 30 | std::vector Nearest( std::vector< std::vector > , std::vector ); 31 | 32 | //Steer function prototype 33 | std::vector Steer( std::vector, std::vector, float ); 34 | 35 | //gridValue function prototype 36 | int gridValue(nav_msgs::OccupancyGrid &,std::vector); 37 | 38 | //ObstacleFree function prototype 39 | char ObstacleFree(std::vector , std::vector & , nav_msgs::OccupancyGrid); 40 | 41 | bool getCompleteFrontier(geometry_msgs::Point &, geometry_msgs::PointStamped &, nav_msgs::OccupancyGrid&); 42 | 43 | std::vector nhood8(unsigned int, nav_msgs::OccupancyGrid&); 44 | 45 | 46 | bool isNewFrontierCell(unsigned, nav_msgs::OccupancyGrid&, const std::vector&); 47 | 48 | std::vector pointOfIndex(nav_msgs::OccupancyGrid&, unsigned int); 49 | 50 | #endif 51 | 52 | -------------------------------------------------------------------------------- /launch/simple.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /launch/single.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /scripts/getfrontier.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | #--------Include modules--------------- 5 | from copy import copy 6 | import rospy 7 | from nav_msgs.msg import OccupancyGrid 8 | 9 | import numpy as np 10 | import cv2 11 | 12 | #----------------------------------------------------- 13 | 14 | def getfrontier(mapData): 15 | data=mapData.data 16 | w=mapData.info.width 17 | h=mapData.info.height 18 | resolution=mapData.info.resolution 19 | Xstartx=mapData.info.origin.position.x 20 | Xstarty=mapData.info.origin.position.y 21 | 22 | img = np.zeros((h, w, 1), np.uint8) 23 | 24 | for i in range(0,h): 25 | for j in range(0,w): 26 | if data[i*w+j]==100: 27 | img[i,j]=0 28 | elif data[i*w+j]==0: 29 | img[i,j]=255 30 | elif data[i*w+j]==-1: 31 | img[i,j]=205 32 | 33 | 34 | o=cv2.inRange(img,0,1) 35 | edges = cv2.Canny(img,0,255) 36 | im2, contours, hierarchy = cv2.findContours(o,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 37 | cv2.drawContours(o, contours, -1, (255,255,255), 5) 38 | o=cv2.bitwise_not(o) 39 | res = cv2.bitwise_and(o,edges) 40 | #------------------------------ 41 | 42 | frontier=copy(res) 43 | im2, contours, hierarchy = cv2.findContours(frontier,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 44 | cv2.drawContours(frontier, contours, -1, (255,255,255), 2) 45 | 46 | im2, contours, hierarchy = cv2.findContours(frontier,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 47 | all_pts=[] 48 | if len(contours)>0: 49 | upto=len(contours)-1 50 | i=0 51 | maxx=0 52 | maxind=0 53 | 54 | for i in range(0,len(contours)): 55 | cnt = contours[i] 56 | M = cv2.moments(cnt) 57 | cx = int(M['m10']/M['m00']) 58 | cy = int(M['m01']/M['m00']) 59 | xr=cx*resolution+Xstartx 60 | yr=cy*resolution+Xstarty 61 | pt=[np.array([xr,yr])] 62 | if len(all_pts)>0: 63 | all_pts=np.vstack([all_pts,pt]) 64 | else: 65 | 66 | all_pts=pt 67 | 68 | return all_pts 69 | 70 | 71 | -------------------------------------------------------------------------------- /src/mtrand.cpp: -------------------------------------------------------------------------------- 1 | // mtrand.cpp, see include file mtrand.h for information 2 | 3 | #include "mtrand.h" 4 | // non-inline function definitions and static member definitions cannot 5 | // reside in header file because of the risk of multiple declarations 6 | 7 | // initialization of static private members 8 | unsigned long MTRand_int32::state[n] = {0x0UL}; 9 | int MTRand_int32::p = 0; 10 | bool MTRand_int32::init = false; 11 | 12 | void MTRand_int32::gen_state() { // generate new state vector 13 | for (int i = 0; i < (n - m); ++i) 14 | state[i] = state[i + m] ^ twiddle(state[i], state[i + 1]); 15 | for (int i = n - m; i < (n - 1); ++i) 16 | state[i] = state[i + m - n] ^ twiddle(state[i], state[i + 1]); 17 | state[n - 1] = state[m - 1] ^ twiddle(state[n - 1], state[0]); 18 | p = 0; // reset position 19 | } 20 | 21 | void MTRand_int32::seed(unsigned long s) { // init by 32 bit seed 22 | state[0] = s & 0xFFFFFFFFUL; // for > 32 bit machines 23 | for (int i = 1; i < n; ++i) { 24 | state[i] = 1812433253UL * (state[i - 1] ^ (state[i - 1] >> 30)) + i; 25 | // see Knuth TAOCP Vol2. 3rd Ed. P.106 for multiplier 26 | // in the previous versions, MSBs of the seed affect only MSBs of the array state 27 | // 2002/01/09 modified by Makoto Matsumoto 28 | state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines 29 | } 30 | p = n; // force gen_state() to be called for next random number 31 | } 32 | 33 | void MTRand_int32::seed(const unsigned long* array, int size) { // init by array 34 | seed(19650218UL); 35 | int i = 1, j = 0; 36 | for (int k = ((n > size) ? n : size); k; --k) { 37 | state[i] = (state[i] ^ ((state[i - 1] ^ (state[i - 1] >> 30)) * 1664525UL)) 38 | + array[j] + j; // non linear 39 | state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines 40 | ++j; j %= size; 41 | if ((++i) == n) { state[0] = state[n - 1]; i = 1; } 42 | } 43 | for (int k = n - 1; k; --k) { 44 | state[i] = (state[i] ^ ((state[i - 1] ^ (state[i - 1] >> 30)) * 1566083941UL)) - i; 45 | state[i] &= 0xFFFFFFFFUL; // for > 32 bit machines 46 | if ((++i) == n) { state[0] = state[n - 1]; i = 1; } 47 | } 48 | state[0] = 0x80000000UL; // MSB is 1; assuring non-zero initial array 49 | p = n; // force gen_state() to be called for next random number 50 | } 51 | -------------------------------------------------------------------------------- /scripts/recordDistance_two_robots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import Point 6 | from math import sqrt 7 | from copy import copy 8 | 9 | class RecordDistance(): 10 | 11 | def __init__(self): 12 | 13 | # init and name the node 14 | rospy.init_node('recordDistanceNode', anonymous=False) 15 | 16 | self.listener = tf.TransformListener() 17 | 18 | rateHz = 1 19 | 20 | rate = rospy.Rate(rateHz) 21 | 22 | robots = ['robot_1', 'robot_2'] 23 | 24 | distances = [0, 0] 25 | last = [] 26 | p = Point() 27 | p.x = 0 28 | p.y = 0 29 | for i in range(len(distances)): 30 | last.append(copy(p)) 31 | 32 | for i in range(len(robots)): 33 | temp = 1 34 | while temp != 0: 35 | try: 36 | self.listener.waitForTransform('/'+ robots[i]+'/odom','/'+ robots[i]+'/base_link', rospy.Time(), rospy.Duration(5)) 37 | temp = 0 38 | except(tf.Exception): 39 | rospy.logerr("can't get the odom frame of robots" + str(i)) 40 | 41 | 42 | # rospy.signal_shutdown('tf Exception') 43 | 44 | while not rospy.is_shutdown(): 45 | dist = 0 46 | for i in range(len(robots)): 47 | position = self.get_odom(robots[i]) 48 | distances[i] += sqrt((position.x-last[i].x)**2 + (position.y-last[i].y)**2) 49 | dist += distances[i] 50 | last[i].x = position.x 51 | last[i].y = position.y 52 | 53 | rospy.logwarn('robots'+str(i)+' distance: ' + str(distances[i])) 54 | rospy.logwarn('Total distance: ' + str(dist)) 55 | rate.sleep() 56 | # rospy.sleep(5) 57 | 58 | def get_odom(self, robot): 59 | # get the current transform between the odom and base frame 60 | try: 61 | (trans, rot) = self.listener.lookupTransform('/'+ robot+'/odom', '/'+robot+'/base_link', rospy.Time(0)) 62 | except (tf.Exception): 63 | rospy.ERROR('tf Exception') 64 | return 65 | return Point(*trans) 66 | 67 | if __name__ == '__main__': 68 | try: 69 | RecordDistance() 70 | except (Exception): 71 | rospy.loginfo("Terminated.") 72 | -------------------------------------------------------------------------------- /scripts/recordDistance.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import Point 6 | from math import sqrt 7 | from copy import copy 8 | 9 | class RecordDistance(): 10 | 11 | def __init__(self): 12 | 13 | # init and name the node 14 | rospy.init_node('recordDistanceNode', anonymous=False) 15 | 16 | self.listener = tf.TransformListener() 17 | 18 | rateHz = 0.2 19 | 20 | rate = rospy.Rate(rateHz) 21 | 22 | robots = ['robot_1', 'robot_2', 'robot_3'] 23 | 24 | distances = [0, 0, 0] 25 | last = [] 26 | p = Point() 27 | p.x = 0 28 | p.y = 0 29 | for i in range(len(distances)): 30 | last.append(copy(p)) 31 | 32 | for i in range(len(robots)): 33 | temp = 1 34 | while temp != 0: 35 | try: 36 | self.listener.waitForTransform('/'+ robots[i]+'/odom','/'+ robots[i]+'/base_link', rospy.Time(), rospy.Duration(5)) 37 | temp = 0 38 | except(tf.Exception): 39 | rospy.logerr("can't get the odom frame of robots" + str(i)) 40 | 41 | 42 | # rospy.signal_shutdown('tf Exception') 43 | 44 | while not rospy.is_shutdown(): 45 | dist = 0 46 | for i in range(len(robots)): 47 | position = self.get_odom(robots[i]) 48 | distances[i] += sqrt((position.x-last[i].x)**2 + (position.y-last[i].y)**2) 49 | dist += distances[i] 50 | last[i].x = position.x 51 | last[i].y = position.y 52 | 53 | rospy.logwarn('robots'+str(i)+' distance: ' + str(distances[i])) 54 | rospy.logwarn('Total distance: ' + str(dist)) 55 | # rate.sleep() 56 | rospy.sleep(5) 57 | 58 | def get_odom(self, robot): 59 | # get the current transform between the odom and base frame 60 | try: 61 | (trans, rot) = self.listener.lookupTransform('/'+ robot+'/odom', '/'+robot+'/base_link', rospy.Time(0)) 62 | except (tf.Exception): 63 | rospy.ERROR('tf Exception') 64 | return 65 | return Point(*trans) 66 | 67 | if __name__ == '__main__': 68 | try: 69 | RecordDistance() 70 | except (Exception): 71 | rospy.loginfo("Terminated.") 72 | -------------------------------------------------------------------------------- /scripts/recordDistance_two_robots_real.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import Point 6 | from math import sqrt 7 | from copy import copy 8 | 9 | class RecordDistance(): 10 | 11 | def __init__(self): 12 | 13 | # init and name the node 14 | rospy.init_node('recordDistanceNode', anonymous=False) 15 | 16 | self.listener = tf.TransformListener() 17 | 18 | rateHz = 0.5 19 | 20 | rate = rospy.Rate(rateHz) 21 | 22 | robots = ['robot_0', 'robot_1'] 23 | 24 | distances = [0, 0] 25 | last = [] 26 | p = Point() 27 | p.x = 0 28 | p.y = 0 29 | for i in range(len(distances)): 30 | last.append(copy(p)) 31 | 32 | for i in range(len(robots)): 33 | temp = 1 34 | while temp != 0: 35 | try: 36 | self.listener.waitForTransform('/'+ robots[i]+'/odom','/'+ robots[i]+'/base_footprint', rospy.Time(), rospy.Duration(5)) 37 | temp = 0 38 | except(tf.Exception): 39 | rospy.logerr("can't get the odom frame of robots" + str(i)) 40 | 41 | 42 | # rospy.signal_shutdown('tf Exception') 43 | 44 | while not rospy.is_shutdown(): 45 | dist = 0 46 | for i in range(len(robots)): 47 | position = self.get_odom(robots[i]) 48 | distances[i] += sqrt((position.x-last[i].x)**2 + (position.y-last[i].y)**2) 49 | dist += distances[i] 50 | last[i].x = position.x 51 | last[i].y = position.y 52 | 53 | rospy.logwarn('robots'+str(i)+' distance: ' + str(distances[i])) 54 | rospy.logwarn('Total distance: ' + str(dist)) 55 | rate.sleep() 56 | # rospy.sleep(5) 57 | 58 | def get_odom(self, robot): 59 | # get the current transform between the odom and base frame 60 | try: 61 | (trans, rot) = self.listener.lookupTransform('/'+ robot+'/odom', '/'+robot+'/base_footprint', rospy.Time(0)) 62 | except (tf.Exception): 63 | rospy.ERROR('tf Exception') 64 | return 65 | return Point(*trans) 66 | 67 | if __name__ == '__main__': 68 | try: 69 | RecordDistance() 70 | except (Exception): 71 | rospy.loginfo("Terminated.") 72 | -------------------------------------------------------------------------------- /launch/three_tb3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # multi_rrt_exploration 2 | 3 | This package is adapted from rrt_exploration. You could run the following command on the terminal: 4 | ```sh 5 | $ roslaunch multi_rrt_exploration rrt_three_robots.launch 6 | ``` 7 | 8 | #### 介绍 9 | 该功能包改编于GitHub上的开源项目[rrt_exploration](https://github.com/hasauino/rrt_exploration)，笔者已在ROS melodic下跑通（采用三台turtlebot3机器人测试），整体来看，该项目中所使用的RRT算法扩展性与稳定性较好，在计算机算力允许的范围内，能够扩展至更多的机器人数量。 10 | 11 | #### 软件依赖 12 | 要在ROS melodic上成功运行该功能包并非一件容易的事，笔者花费了1周时间才将其从kinetic成功移植到melodic中，所需依赖如下： 13 | 1. 地图融合：[m-explore](https://github.com/hrnr/m-explore) 14 | 2. 探索插件：[frontier_exploration](https://github.com/paulbovbel/frontier_exploration) 15 | 3. 机器人模型：[turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) 16 | 4. [turtlebot3_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_msgs) 17 | 5. Python库：OpenCV、Numpy、Sklearn 18 | 19 | ```sh 20 | $ sudo apt-get install python-opencv 21 | $ sudo apt-get install python-numpy 22 | $ sudo apt-get install python-scikits-learn 23 | ``` 24 | 25 | #### 使用说明 26 | 1. 仿真: 27 | ```sh 28 | $ roslaunch multi_rrt_exploration rrt_three_robots.launch 29 | ``` 30 | 31 | 2. 实物（导航）: 32 | ```sh 33 | master: roscore 34 | robot_0: export ROS_NAMESPACE=robot_0 35 | robot_0: roslaunch rikirobot lidar_slam_multi.launch tf_prefix:=robot_0 36 | robot_1: export ROS_NAMESPACE=robot_1 37 | robot_1: roslaunch robot_navigation robot_slam_laser_multi.launch tf_prefix:=robot_1 38 | master: roslaunch multirobot_test robots_merge.launch 39 | ``` 40 | 41 | 3. 实物（探索）: 42 | ```sh 43 | master: roslaunch multirobot_test robots_merge.launch 44 | robot_: export ROS_NAMESPACE=robot_ 45 | robot_: roslaunch rikirobot lidar_slam_multi.launch tf_prefix:=robot_ 46 | master: roslaunch explore_lite explore.launch 47 | ``` 48 | 49 | ### 修改说明: 50 | 1. robot_: 配置 launch文件中加入 move_base explore_robot_.launch 配置使用公共 /map 话题 51 | 2. 修改 move_base global_costmap_params.yaml 、local_costmap_params.yaml中坐标系 52 | 53 | ## 部分实验结果： 54 | 55 | #### 多机器人协同探索实验 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /launch/three_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /launch/rrt_three_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /launch/two_robots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /launch/two_robots_real.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /scripts/frontier_opencv_detector.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | #--------Include modules--------------- 5 | import rospy 6 | from visualization_msgs.msg import Marker 7 | from nav_msgs.msg import OccupancyGrid 8 | from geometry_msgs.msg import PointStamped 9 | from getfrontier import getfrontier 10 | 11 | #----------------------------------------------------- 12 | # Subscribers' callbacks------------------------------ 13 | mapData=OccupancyGrid() 14 | 15 | 16 | def mapCallBack(data): 17 | global mapData 18 | mapData=data 19 | 20 | 21 | 22 | 23 | # Node---------------------------------------------- 24 | def node(): 25 | global mapData 26 | exploration_goal=PointStamped() 27 | map_topic= rospy.get_param('~map_topic','robot_1/map') 28 | rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) 29 | targetspub = rospy.Publisher('/detected_points', PointStamped, queue_size=10) 30 | pub = rospy.Publisher('shapes', Marker, queue_size=10) 31 | rospy.init_node('detector', anonymous=False) 32 | # wait until map is received, when a map is received, mapData.header.seq will not be < 1 33 | while mapData.header.seq<1 or len(mapData.data)<1: 34 | pass 35 | 36 | rate = rospy.Rate(50) 37 | points=Marker() 38 | 39 | #Set the frame ID and timestamp. See the TF tutorials for information on these. 40 | points.header.frame_id=mapData.header.frame_id 41 | points.header.stamp=rospy.Time.now() 42 | 43 | points.ns= "markers" 44 | points.id = 0 45 | 46 | points.type = Marker.POINTS 47 | #Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 48 | points.action = Marker.ADD; 49 | 50 | points.pose.orientation.w = 1.0; 51 | points.scale.x=points.scale.y=0.3; 52 | points.color.r = 255.0/255.0 53 | points.color.g = 0.0/255.0 54 | points.color.b = 0.0/255.0 55 | points.color.a=1; 56 | points.lifetime == rospy.Duration(); 57 | 58 | #-------------------------------OpenCV frontier detection------------------------------------------ 59 | while not rospy.is_shutdown(): 60 | frontiers=getfrontier(mapData) 61 | for i in range(len(frontiers)): 62 | x=frontiers[i] 63 | exploration_goal.header.frame_id= mapData.header.frame_id 64 | exploration_goal.header.stamp=rospy.Time(0) 65 | exploration_goal.point.x=x[0] 66 | exploration_goal.point.y=x[1] 67 | exploration_goal.point.z=0 68 | 69 | 70 | targetspub.publish(exploration_goal) 71 | points.points=[exploration_goal.point] 72 | pub.publish(points) 73 | rate.sleep() 74 | 75 | 76 | 77 | #rate.sleep() 78 | 79 | 80 | 81 | #_____________________________________________________________________________ 82 | 83 | if __name__ == '__main__': 84 | try: 85 | node() 86 | except rospy.ROSInterruptException: 87 | pass 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /src/functions.cpp: -------------------------------------------------------------------------------- 1 | #include "functions.h" 2 | 3 | 4 | // rdm class, for gentaring random flot numbers 5 | rdm::rdm() {i=time(0);} 6 | float rdm::randomize() { i=i+1; srand (i); return float(rand())/float(RAND_MAX);} 7 | 8 | 9 | 10 | //Norm function 11 | float Norm(std::vector x1,std::vector x2) 12 | { 13 | return pow( (pow((x2[0]-x1[0]),2)+pow((x2[1]-x1[1]),2)) ,0.5); 14 | } 15 | 16 | 17 | //sign function 18 | float sign(float n) 19 | { 20 | if (n<0.0){return -1.0;} 21 | else{return 1.0;} 22 | } 23 | 24 | 25 | //Nearest function 26 | std::vector Nearest( std::vector< std::vector > V, std::vector x){ 27 | 28 | float min=Norm(V[0],x); 29 | int min_index; 30 | float temp; 31 | 32 | for (int j=0;j Steer( std::vector x_nearest , std::vector x_rand, float eta){ 48 | std::vector x_new; 49 | 50 | if (Norm(x_nearest,x_rand)<=eta){ 51 | x_new=x_rand; 52 | } 53 | else{ 54 | 55 | 56 | float m=(x_rand[1]-x_nearest[1])/(x_rand[0]-x_nearest[0]); 57 | 58 | x_new.push_back( (sign(x_rand[0]-x_nearest[0]))* ( sqrt( (pow(eta,2)) / ((pow(m,2))+1) ) )+x_nearest[0] ); 59 | x_new.push_back( m*(x_new[0]-x_nearest[0])+x_nearest[1] ); 60 | 61 | if(x_rand[0]==x_nearest[0]){ 62 | x_new[0]=x_nearest[0]; 63 | x_new[1]=x_nearest[1]+eta; 64 | } 65 | 66 | 67 | 68 | } 69 | return x_new; 70 | } 71 | 72 | 73 | 74 | 75 | 76 | //gridValue function 77 | int gridValue(nav_msgs::OccupancyGrid &mapData,std::vector Xp){ 78 | 79 | float resolution=mapData.info.resolution; 80 | float Xstartx=mapData.info.origin.position.x; 81 | float Xstarty=mapData.info.origin.position.y; 82 | 83 | float width=mapData.info.width; 84 | std::vector Data=mapData.data; 85 | 86 | //returns grid value at "Xp" location 87 | //map data: 100 occupied -1 unknown 0 free 88 | float indx=( floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) ); 89 | int out; 90 | out=Data[int(indx)]; 91 | return out; 92 | } 93 | 94 | 95 | 96 | 97 | // ObstacleFree function------------------------------------- 98 | 99 | char ObstacleFree(std::vector xnear, std::vector &xnew, nav_msgs::OccupancyGrid mapsub){ 100 | float rez=float(mapsub.info.resolution)*.2; 101 | float stepz=int(ceil(Norm(xnew,xnear))/rez); 102 | std::vector xi=xnear; 103 | char obs=0; char unk=0; 104 | 105 | geometry_msgs::Point p; 106 | for (int c=0;c out = nhood8(indx, mapsub); 140 | 141 | // center of the frontier 142 | float centerx = 0; 143 | float centery = 0; 144 | unsigned int size = 1; 145 | 146 | 147 | std::vector data = mapsub.data; 148 | 149 | if (data[indx] != -1) { 150 | ROS_WARN("The real map is not match with the gridmap!"); 151 | return false; 152 | } 153 | 154 | std::queue queue; 155 | std::vector visited(width*height, false); 156 | 157 | // push the initial cell 158 | queue.push(indx); 159 | visited[indx] = true; 160 | 161 | while (!queue.empty()) { 162 | unsigned int idx = queue.front(); 163 | queue.pop(); 164 | 165 | for (unsigned int nbr : nhood8(idx, mapsub)) { 166 | if (isNewFrontierCell(nbr, mapsub, visited)) { 167 | visited[nbr] = true; 168 | queue.push(nbr); 169 | 170 | std::vector point = pointOfIndex(mapsub, nbr); 171 | centerx += point[0]; 172 | centery += point[1]; 173 | size++; 174 | 175 | //std::cout<<"-----------------------\n" << point[0] << "," << point[1]<< std::endl; 176 | 177 | } 178 | } 179 | 180 | 181 | } 182 | 183 | exploration_goal.point.x = centerx/size; 184 | exploration_goal.point.y = centery/size; 185 | 186 | return true; 187 | } 188 | 189 | std::vector nhood8(unsigned int indx, nav_msgs::OccupancyGrid& mapData) { 190 | 191 | //returns grid value at "Xp" location 192 | //map data: 100 occupied -1 unknown 0 free 193 | unsigned int width = mapData.info.width; 194 | unsigned int height = mapData.info.height; 195 | //float indx=( floor((p.y-Xstarty)/resolution)*width)+( floor((p.x-Xstartx)/resolution) ); 196 | std::vector out; 197 | //std::vector Xp; 198 | //Xp.push_back(p.x); 199 | 200 | if (indx > width * height - 1) { 201 | ROS_WARN("Evaluating nhood for offmap point"); 202 | return out; 203 | } 204 | 205 | if (indx % width > 0) { 206 | out.push_back(indx-1); 207 | } 208 | 209 | if (indx % width < (width-1)) { 210 | out.push_back(indx+1); 211 | } 212 | 213 | if (indx >= width) { 214 | out.push_back(indx - width); 215 | } 216 | 217 | if (indx < width * (height-1)) { 218 | out.push_back(indx+width); 219 | } 220 | 221 | if (indx % width > 0 && indx >= width) { 222 | out.push_back(indx-1-width); 223 | } 224 | 225 | if (indx % width > 0 && indx < width*(height-1)){ 226 | out.push_back(indx-1+width); 227 | } 228 | 229 | if (indx % width < (width -1) && indx>=width) { 230 | out.push_back(indx+1-width); 231 | } 232 | 233 | if (indx % width < (width-1) && indx < width*(height-1)) { 234 | out.push_back(indx+1+width); 235 | } 236 | 237 | return out; 238 | } 239 | 240 | 241 | bool isNewFrontierCell(unsigned indx, nav_msgs::OccupancyGrid& mapData, const std::vector& visited) { 242 | std::vector data = mapData.data; 243 | 244 | //map data: 100 occupied -1 unknown 0 free 245 | if (data[indx] != -1 || visited[indx]) { 246 | return false; 247 | } 248 | 249 | for (unsigned int nbr : nhood8(indx, mapData)) { 250 | if (data[nbr] == 0) { 251 | return true; 252 | } 253 | } 254 | 255 | return false; 256 | } 257 | 258 | std::vector pointOfIndex(nav_msgs::OccupancyGrid& mapData, unsigned int indx){ 259 | std::vector point; 260 | unsigned int width = mapData.info.width; 261 | float Xstartx = mapData.info.origin.position.x; 262 | float Xstarty = mapData.info.origin.position.y; 263 | float resolution = mapData.info.resolution; 264 | 265 | float x = Xstartx + (indx % width)*resolution; 266 | float y = Xstarty + (indx / width)*resolution; 267 | 268 | point.push_back(x); 269 | point.push_back(y); 270 | 271 | return point; 272 | } 273 | -------------------------------------------------------------------------------- /scripts/assigner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #--------Include modules--------------- 4 | from copy import copy 5 | import rospy 6 | from visualization_msgs.msg import Marker 7 | from geometry_msgs.msg import Point 8 | from nav_msgs.msg import OccupancyGrid 9 | import tf 10 | from multi_rrt_exploration.msg import PointArray 11 | from time import time 12 | from numpy import array 13 | from numpy import linalg as LA 14 | from numpy import all as All 15 | from numpy import inf 16 | from functions import robot,informationGain,discount 17 | from numpy.linalg import norm 18 | import time 19 | 20 | # Subscribers' callbacks------------------------------ 21 | mapData=OccupancyGrid() 22 | frontiers=[] 23 | global1=OccupancyGrid() 24 | global2=OccupancyGrid() 25 | global3=OccupancyGrid() 26 | globalmaps=[] 27 | def callBack(data): 28 | global frontiers 29 | frontiers=[] 30 | for point in data.points: 31 | frontiers.append(array([point.x,point.y])) 32 | 33 | def mapCallBack(data): 34 | global mapData 35 | mapData=data 36 | # Node---------------------------------------------- 37 | 38 | def node(): 39 | 40 | stop = 0 41 | 42 | global frontiers,mapData,global1,global2,global3,globalmaps 43 | rospy.init_node('assigner', anonymous=False) 44 | 45 | # fetching all parameters 46 | map_topic= rospy.get_param('~map_topic','/map_merge/map') 47 | info_radius= rospy.get_param('~info_radius',1.0) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate 48 | info_multiplier=rospy.get_param('~info_multiplier',3.0) 49 | hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0) #at least as much as the laser scanner range 50 | hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region 51 | frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points') 52 | n_robots = rospy.get_param('~n_robots',1) 53 | namespace = rospy.get_param('~namespace','') 54 | namespace_init_count = rospy.get_param('~namespace_init_count',1) 55 | delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5) 56 | rateHz = rospy.get_param('~rate',2) 57 | 58 | rate = rospy.Rate(rateHz) 59 | #------------------------------------------- 60 | rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) 61 | rospy.Subscriber(frontiers_topic, PointArray, callBack) 62 | #--------------------------------------------------------------------------------------------------------------- 63 | # wait if no frontier is received yet 64 | while len(frontiers)<1: 65 | pass 66 | centroids=copy(frontiers) 67 | #wait if map is not received yet 68 | while (len(mapData.data)<1): 69 | pass 70 | 71 | robots=[] 72 | if len(namespace)>0: 73 | for i in range(0,n_robots): 74 | robots.append(robot(namespace+str(i+namespace_init_count))) 75 | elif len(namespace)==0: 76 | robots.append(robot(namespace)) 77 | for i in range(0,n_robots): 78 | robots[i].sendGoal(robots[i].getPosition()) 79 | 80 | 81 | start = time.time() 82 | #------------------------------------------------------------------------- 83 | #-------------------- Initial Rotation -------------------------- 84 | 85 | for i in range(0,n_robots): 86 | robots[i].sendGoal(robots[i].getPosition(), rotation=True) 87 | rospy.sleep(3); 88 | rospy.loginfo("initial finished.") 89 | 90 | 91 | #------------------------------------------------------------------------- 92 | #--------------------- Main Loop ------------------------------- 93 | #------------------------------------------------------------------------- 94 | while not rospy.is_shutdown(): 95 | centroids=copy(frontiers) 96 | #------------------------------------------------------------------------- 97 | #Get information gain for each frontier point 98 | infoGain=[] 99 | for ip in range(0,len(centroids)): 100 | infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)) 101 | #------------------------------------------------------------------------- 102 | #get number of available/busy robots 103 | na=[] #available robots 104 | nb=[] #busy robots 105 | for i in range(0,n_robots): 106 | if (robots[i].getState()==1): 107 | nb.append(i) 108 | else: 109 | na.append(i) 110 | rospy.loginfo("available robots: "+str(na)) 111 | #------------------------------------------------------------------------- 112 | #get dicount and update informationGain 113 | for i in nb+na: 114 | infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius) 115 | #------------------------------------------------------------------------- 116 | revenue_record=[] 117 | centroid_record=[] 118 | id_record=[] 119 | 120 | for ir in na: 121 | for ip in range(0,len(centroids)): 122 | cost=norm(robots[ir].getPosition()-centroids[ip]) 123 | threshold=1 124 | information_gain=infoGain[ip] 125 | if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): 126 | 127 | information_gain*=hysteresis_gain 128 | revenue=information_gain*info_multiplier-cost 129 | revenue_record.append(revenue) 130 | centroid_record.append(centroids[ip]) 131 | id_record.append(ir) 132 | 133 | if len(na)<1: 134 | revenue_record=[] 135 | centroid_record=[] 136 | id_record=[] 137 | for ir in nb: 138 | for ip in range(0,len(centroids)): 139 | cost=norm(robots[ir].getPosition()-centroids[ip]) 140 | threshold=1 141 | information_gain=infoGain[ip] 142 | if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): 143 | information_gain*=hysteresis_gain 144 | 145 | if ((norm(centroids[ip]-robots[ir].assigned_point))0): 159 | stop = 0 160 | winner_id=revenue_record.index(max(revenue_record)) 161 | robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) 162 | rospy.loginfo(namespace+str(namespace_init_count+id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) 163 | rospy.sleep(delay_after_assignement) 164 | else: 165 | stop += 1 166 | 167 | if (stop == rateHz*5): 168 | for i in range(0, n_robots): 169 | robots[i].cancelGoal() 170 | 171 | rospy.loginfo("Exploration has been finished.") 172 | break 173 | #------------------------------------------------------------------------- 174 | rate.sleep() 175 | #------------------------------------------------------------------------- 176 | end = time.time() 177 | rospy.logwarn("total time: " + str(end-start)) 178 | 179 | 180 | if __name__ == '__main__': 181 | try: 182 | node() 183 | except rospy.ROSInterruptException: 184 | pass 185 | 186 | 187 | 188 | 189 | -------------------------------------------------------------------------------- /scripts/assigner_real_two_robots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #--------Include modules--------------- 4 | from copy import copy 5 | import rospy 6 | from visualization_msgs.msg import Marker 7 | from geometry_msgs.msg import Point 8 | from nav_msgs.msg import OccupancyGrid 9 | import tf 10 | from rrt_exploration.msg import PointArray 11 | from time import time 12 | from numpy import array 13 | from numpy import linalg as LA 14 | from numpy import all as All 15 | from numpy import inf 16 | from functions_real_two_robots import robot,informationGain,discount 17 | from numpy.linalg import norm 18 | import time 19 | 20 | # Subscribers' callbacks------------------------------ 21 | mapData=OccupancyGrid() 22 | frontiers=[] 23 | global1=OccupancyGrid() 24 | global2=OccupancyGrid() 25 | global3=OccupancyGrid() 26 | globalmaps=[] 27 | def callBack(data): 28 | global frontiers 29 | frontiers=[] 30 | for point in data.points: 31 | frontiers.append(array([point.x,point.y])) 32 | 33 | def mapCallBack(data): 34 | global mapData 35 | mapData=data 36 | # Node---------------------------------------------- 37 | 38 | def node(): 39 | 40 | stop = 0 41 | 42 | global frontiers,mapData,global1,global2,global3,globalmaps 43 | rospy.init_node('assigner', anonymous=False) 44 | 45 | # fetching all parameters 46 | map_topic= rospy.get_param('~map_topic','/map') 47 | info_radius= rospy.get_param('~info_radius',1.0) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate 48 | info_multiplier=rospy.get_param('~info_multiplier',3.0) 49 | hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0) #at least as much as the laser scanner range 50 | hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region 51 | frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points') 52 | n_robots = rospy.get_param('~n_robots',1) 53 | namespace = rospy.get_param('~namespace','') 54 | namespace_init_count = rospy.get_param('~namespace_init_count',1) 55 | delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5) 56 | rateHz = rospy.get_param('~rate',2) 57 | 58 | rate = rospy.Rate(rateHz) 59 | #------------------------------------------- 60 | rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) 61 | rospy.Subscriber(frontiers_topic, PointArray, callBack) 62 | #--------------------------------------------------------------------------------------------------------------- 63 | # wait if no frontier is received yet 64 | while len(frontiers)<1: 65 | pass 66 | centroids=copy(frontiers) 67 | #wait if map is not received yet 68 | while (len(mapData.data)<1): 69 | pass 70 | 71 | robots=[] 72 | if len(namespace)>0: 73 | for i in range(0,n_robots): 74 | robots.append(robot(namespace+str(i+namespace_init_count))) 75 | elif len(namespace)==0: 76 | robots.append(robot(namespace)) 77 | for i in range(0,n_robots): 78 | robots[i].sendGoal(robots[i].getPosition()) 79 | 80 | 81 | start = time.time() 82 | #------------------------------------------------------------------------- 83 | #-------------------- Initial Rotation -------------------------- 84 | 85 | for i in range(0,n_robots): 86 | robots[i].sendGoal(robots[i].getPosition(), rotation=True) 87 | rospy.sleep(3); 88 | rospy.loginfo("initial finished.") 89 | 90 | 91 | #------------------------------------------------------------------------- 92 | #--------------------- Main Loop ------------------------------- 93 | #------------------------------------------------------------------------- 94 | while not rospy.is_shutdown(): 95 | centroids=copy(frontiers) 96 | #------------------------------------------------------------------------- 97 | #Get information gain for each frontier point 98 | infoGain=[] 99 | for ip in range(0,len(centroids)): 100 | infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)) 101 | #------------------------------------------------------------------------- 102 | #get number of available/busy robots 103 | na=[] #available robots 104 | nb=[] #busy robots 105 | for i in range(0,n_robots): 106 | if (robots[i].getState()==1): 107 | nb.append(i) 108 | else: 109 | na.append(i) 110 | rospy.loginfo("available robots: "+str(na)) 111 | #------------------------------------------------------------------------- 112 | #get dicount and update informationGain 113 | for i in nb+na: 114 | infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius) 115 | #------------------------------------------------------------------------- 116 | revenue_record=[] 117 | centroid_record=[] 118 | id_record=[] 119 | 120 | for ir in na: 121 | for ip in range(0,len(centroids)): 122 | cost=norm(robots[ir].getPosition()-centroids[ip]) 123 | threshold=1 124 | information_gain=infoGain[ip] 125 | if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): 126 | 127 | information_gain*=hysteresis_gain 128 | revenue=information_gain*info_multiplier-cost 129 | revenue_record.append(revenue) 130 | centroid_record.append(centroids[ip]) 131 | id_record.append(ir) 132 | 133 | if len(na)<1: 134 | revenue_record=[] 135 | centroid_record=[] 136 | id_record=[] 137 | for ir in nb: 138 | for ip in range(0,len(centroids)): 139 | cost=norm(robots[ir].getPosition()-centroids[ip]) 140 | threshold=1 141 | information_gain=infoGain[ip] 142 | if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): 143 | information_gain*=hysteresis_gain 144 | 145 | if ((norm(centroids[ip]-robots[ir].assigned_point))0): 159 | stop = 0 160 | winner_id=revenue_record.index(max(revenue_record)) 161 | robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) 162 | rospy.loginfo(namespace+str(namespace_init_count+id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) 163 | rospy.sleep(delay_after_assignement) 164 | else: 165 | stop += 1 166 | 167 | if (stop == rateHz*1000): 168 | for i in range(0, n_robots): 169 | robots[i].cancelGoal() 170 | 171 | rospy.loginfo("Exploration has been finished.") 172 | break 173 | #------------------------------------------------------------------------- 174 | rate.sleep() 175 | #------------------------------------------------------------------------- 176 | end = time.time() 177 | rospy.logwarn("total time: " + str(end-start)) 178 | 179 | 180 | if __name__ == '__main__': 181 | try: 182 | node() 183 | except rospy.ROSInterruptException: 184 | pass 185 | 186 | 187 | 188 | 189 | -------------------------------------------------------------------------------- /src/global_rrt_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "stdint.h" 8 | #include "functions.h" 9 | #include "mtrand.h" 10 | 11 | 12 | #include "nav_msgs/OccupancyGrid.h" 13 | #include "geometry_msgs/PointStamped.h" 14 | #include "std_msgs/Header.h" 15 | #include "nav_msgs/MapMetaData.h" 16 | #include "geometry_msgs/Point.h" 17 | #include "visualization_msgs/Marker.h" 18 | #include 19 | 20 | 21 | 22 | // global variables 23 | nav_msgs::OccupancyGrid mapData; 24 | geometry_msgs::PointStamped clickedpoint; 25 | geometry_msgs::PointStamped exploration_goal; 26 | visualization_msgs::Marker points,line; 27 | float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y; 28 | 29 | rdm r; // for genrating random numbers 30 | 31 | 32 | 33 | //Subscribers callback functions--------------------------------------- 34 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 35 | { 36 | mapData=*msg; 37 | } 38 | 39 | 40 | 41 | void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg) 42 | { 43 | 44 | geometry_msgs::Point p; 45 | p.x=msg->point.x; 46 | p.y=msg->point.y; 47 | p.z=msg->point.z; 48 | 49 | points.points.push_back(p); 50 | 51 | } 52 | 53 | 54 | 55 | 56 | int main(int argc, char **argv) 57 | { 58 | 59 | unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7; 60 | MTRand_int32 irand(init, length); // 32-bit int generator 61 | // this is an example of initializing by an array 62 | // you may use MTRand(seed) with any 32bit integer 63 | // as a seed for a simpler initialization 64 | MTRand drand; // double in [0, 1) generator, already init 65 | 66 | // generate the same numbers as in the original C test program 67 | ros::init(argc, argv, "global_rrt_frontier_detector"); 68 | ros::NodeHandle nh; 69 | 70 | 71 | //debug enabled 72 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info)) { 73 | ros::console::notifyLoggerLevelsChanged(); 74 | } 75 | 76 | // fetching all parameters 77 | float eta,init_map_x,init_map_y,range; 78 | std::string map_topic,base_frame_topic; 79 | 80 | std::string ns; 81 | ns=ros::this_node::getName(); 82 | 83 | ros::param::param(ns+"/eta", eta, 0.5); 84 | ros::param::param(ns+"/map_topic", map_topic, "robot_1/map"); 85 | //--------------------------------------------------------------- 86 | ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack); 87 | ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack); 88 | 89 | ros::Publisher targetspub = nh.advertise("/detected_points", 10); 90 | ros::Publisher pub = nh.advertise(ns+"_shapes", 10); 91 | 92 | ros::Rate rate(100); 93 | 94 | 95 | // wait until map is received, when a map is received, mapData.header.seq will not be < 1 96 | while (mapData.header.seq<1 or mapData.data.size()<1) { ros::spinOnce(); ros::Duration(0.1).sleep();} 97 | 98 | 99 | 100 | //visualizations points and lines.. 101 | points.header.frame_id=mapData.header.frame_id; 102 | line.header.frame_id=mapData.header.frame_id; 103 | points.header.stamp=ros::Time(0); 104 | line.header.stamp=ros::Time(0); 105 | 106 | points.ns=line.ns = "markers"; 107 | points.id = 0; 108 | line.id =1; 109 | 110 | 111 | points.type = points.POINTS; 112 | line.type=line.LINE_LIST; 113 | 114 | //Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 115 | points.action =points.ADD; 116 | line.action = line.ADD; 117 | points.pose.orientation.w =1.0; 118 | line.pose.orientation.w = 1.0; 119 | line.scale.x = 0.03; 120 | line.scale.y= 0.03; 121 | points.scale.x=0.3; 122 | points.scale.y=0.3; 123 | 124 | line.color.r =9.0/255.0; 125 | line.color.g= 91.0/255.0; 126 | line.color.b =236.0/255.0; 127 | points.color.r = 255.0/255.0; 128 | points.color.g = 0.0/255.0; 129 | points.color.b = 0.0/255.0; 130 | points.color.a=1.0; 131 | line.color.a = 1.0; 132 | points.lifetime = ros::Duration(); 133 | line.lifetime = ros::Duration(); 134 | 135 | geometry_msgs::Point p; 136 | 137 | 138 | while(points.points.size()<5) 139 | { 140 | ros::spinOnce(); 141 | 142 | pub.publish(points) ; 143 | } 144 | 145 | 146 | 147 | 148 | std::vector temp1; 149 | temp1.push_back(points.points[0].x); 150 | temp1.push_back(points.points[0].y); 151 | 152 | std::vector temp2; 153 | temp2.push_back(points.points[2].x); 154 | temp2.push_back(points.points[0].y); 155 | 156 | 157 | init_map_x=Norm(temp1,temp2); 158 | temp1.clear(); temp2.clear(); 159 | 160 | temp1.push_back(points.points[0].x); 161 | temp1.push_back(points.points[0].y); 162 | 163 | temp2.push_back(points.points[0].x); 164 | temp2.push_back(points.points[2].y); 165 | 166 | init_map_y=Norm(temp1,temp2); 167 | temp1.clear(); temp2.clear(); 168 | 169 | Xstartx=(points.points[0].x+points.points[2].x)*.5; 170 | Xstarty=(points.points[0].y+points.points[2].y)*.5; 171 | 172 | 173 | 174 | 175 | 176 | geometry_msgs::Point trans; 177 | trans=points.points[4]; 178 | std::vector< std::vector > V; 179 | std::vector xnew; 180 | xnew.push_back( trans.x);xnew.push_back( trans.y); 181 | V.push_back(xnew); 182 | 183 | points.points.clear(); 184 | pub.publish(points) ; 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | std::vector frontiers; 193 | int i=0; 194 | float xr,yr; 195 | std::vector x_rand,x_nearest,x_new; 196 | 197 | 198 | // Main loop 199 | while (ros::ok()){ 200 | 201 | 202 | // Sample free 203 | x_rand.clear(); 204 | xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx; 205 | yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty; 206 | 207 | 208 | x_rand.push_back( xr ); x_rand.push_back( yr ); 209 | 210 | 211 | // Nearest 212 | x_nearest=Nearest(V,x_rand); 213 | 214 | // Steer 215 | 216 | x_new=Steer(x_nearest,x_rand,eta); 217 | 218 | 219 | // ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle 220 | char checking=ObstacleFree(x_nearest,x_new,mapData); 221 | 222 | if (checking==-1){ 223 | //geometry_msgs::PointStamped exploration_point; 224 | 225 | // the first point of a frontier 226 | p.x=x_new[0]; 227 | p.y=x_new[1]; 228 | p.z=0.0; 229 | 230 | ROS_DEBUG("before(%f, %f)", p.x, p.y); 231 | bool existFrontier = getCompleteFrontier(p, exploration_goal, mapData); 232 | if (!existFrontier) { 233 | continue; 234 | } 235 | 236 | exploration_goal.header.stamp=ros::Time(0); 237 | exploration_goal.header.frame_id=mapData.header.frame_id; 238 | //exploration_goal.point.x=x_new[0]; 239 | //exploration_goal.point.y=x_new[1]; 240 | exploration_goal.point.z=0.0; 241 | 242 | //p.x = exploration_goal.point.x; 243 | //p.y = exploration_goal.point.y; 244 | points.points.push_back(p); 245 | 246 | ROS_DEBUG("after(%f, %f)", p.x, p.y); 247 | 248 | pub.publish(points) ; 249 | targetspub.publish(exploration_goal); 250 | points.points.clear(); 251 | 252 | } 253 | 254 | 255 | else if (checking==1){ 256 | V.push_back(x_new); 257 | 258 | p.x=x_new[0]; 259 | p.y=x_new[1]; 260 | p.z=0.0; 261 | line.points.push_back(p); 262 | p.x=x_nearest[0]; 263 | p.y=x_nearest[1]; 264 | p.z=0.0; 265 | line.points.push_back(p); 266 | 267 | } 268 | 269 | 270 | 271 | pub.publish(line); 272 | 273 | 274 | 275 | 276 | ros::spinOnce(); 277 | rate.sleep(); 278 | }return 0;} 279 | -------------------------------------------------------------------------------- /src/global_rrt_detector_real_two_robots.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "stdint.h" 8 | #include "functions.h" 9 | #include "mtrand.h" 10 | 11 | 12 | #include "nav_msgs/OccupancyGrid.h" 13 | #include "geometry_msgs/PointStamped.h" 14 | #include "std_msgs/Header.h" 15 | #include "nav_msgs/MapMetaData.h" 16 | #include "geometry_msgs/Point.h" 17 | #include "visualization_msgs/Marker.h" 18 | #include 19 | 20 | 21 | 22 | // global variables 23 | nav_msgs::OccupancyGrid mapData; 24 | geometry_msgs::PointStamped clickedpoint; 25 | geometry_msgs::PointStamped exploration_goal; 26 | visualization_msgs::Marker points,line; 27 | float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y; 28 | 29 | rdm r; // for genrating random numbers 30 | 31 | 32 | 33 | //Subscribers callback functions--------------------------------------- 34 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 35 | { 36 | mapData=*msg; 37 | } 38 | 39 | 40 | 41 | void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg) 42 | { 43 | 44 | geometry_msgs::Point p; 45 | p.x=msg->point.x; 46 | p.y=msg->point.y; 47 | p.z=msg->point.z; 48 | 49 | points.points.push_back(p); 50 | 51 | } 52 | 53 | 54 | 55 | 56 | int main(int argc, char **argv) 57 | { 58 | 59 | unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7; 60 | MTRand_int32 irand(init, length); // 32-bit int generator 61 | // this is an example of initializing by an array 62 | // you may use MTRand(seed) with any 32bit integer 63 | // as a seed for a simpler initialization 64 | MTRand drand; // double in [0, 1) generator, already init 65 | 66 | // generate the same numbers as in the original C test program 67 | ros::init(argc, argv, "global_rrt_frontier_detector"); 68 | ros::NodeHandle nh; 69 | 70 | 71 | //debug enabled 72 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info)) { 73 | ros::console::notifyLoggerLevelsChanged(); 74 | } 75 | 76 | // fetching all parameters 77 | float eta,init_map_x,init_map_y,range; 78 | std::string map_topic,base_frame_topic; 79 | 80 | std::string ns; 81 | ns=ros::this_node::getName(); 82 | 83 | ros::param::param(ns+"/eta", eta, 0.5); 84 | ros::param::param(ns+"/map_topic", map_topic, "/robot_1/map"); 85 | //--------------------------------------------------------------- 86 | ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack); 87 | ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack); 88 | 89 | ros::Publisher targetspub = nh.advertise("/detected_points", 10); 90 | ros::Publisher pub = nh.advertise(ns+"_shapes", 10); 91 | 92 | ros::Rate rate(100); 93 | 94 | 95 | // wait until map is received, when a map is received, mapData.header.seq will not be < 1 96 | while (mapData.header.seq<1 or mapData.data.size()<1) { ros::spinOnce(); ros::Duration(0.1).sleep();} 97 | 98 | 99 | 100 | //visualizations points and lines.. 101 | points.header.frame_id=mapData.header.frame_id; 102 | line.header.frame_id=mapData.header.frame_id; 103 | points.header.stamp=ros::Time(0); 104 | line.header.stamp=ros::Time(0); 105 | 106 | points.ns=line.ns = "markers"; 107 | points.id = 0; 108 | line.id =1; 109 | 110 | 111 | points.type = points.POINTS; 112 | line.type=line.LINE_LIST; 113 | 114 | //Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 115 | points.action =points.ADD; 116 | line.action = line.ADD; 117 | points.pose.orientation.w =1.0; 118 | line.pose.orientation.w = 1.0; 119 | line.scale.x = 0.03; 120 | line.scale.y= 0.03; 121 | points.scale.x=0.3; 122 | points.scale.y=0.3; 123 | 124 | line.color.r =9.0/255.0; 125 | line.color.g= 91.0/255.0; 126 | line.color.b =236.0/255.0; 127 | points.color.r = 255.0/255.0; 128 | points.color.g = 0.0/255.0; 129 | points.color.b = 0.0/255.0; 130 | points.color.a=1.0; 131 | line.color.a = 1.0; 132 | points.lifetime = ros::Duration(); 133 | line.lifetime = ros::Duration(); 134 | 135 | geometry_msgs::Point p; 136 | 137 | 138 | while(points.points.size()<5) 139 | { 140 | ros::spinOnce(); 141 | 142 | pub.publish(points) ; 143 | } 144 | 145 | 146 | 147 | 148 | std::vector temp1; 149 | temp1.push_back(points.points[0].x); 150 | temp1.push_back(points.points[0].y); 151 | 152 | std::vector temp2; 153 | temp2.push_back(points.points[2].x); 154 | temp2.push_back(points.points[0].y); 155 | 156 | 157 | init_map_x=Norm(temp1,temp2); 158 | temp1.clear(); temp2.clear(); 159 | 160 | temp1.push_back(points.points[0].x); 161 | temp1.push_back(points.points[0].y); 162 | 163 | temp2.push_back(points.points[0].x); 164 | temp2.push_back(points.points[2].y); 165 | 166 | init_map_y=Norm(temp1,temp2); 167 | temp1.clear(); temp2.clear(); 168 | 169 | Xstartx=(points.points[0].x+points.points[2].x)*.5; 170 | Xstarty=(points.points[0].y+points.points[2].y)*.5; 171 | 172 | 173 | 174 | 175 | 176 | geometry_msgs::Point trans; 177 | trans=points.points[4]; 178 | std::vector< std::vector > V; 179 | std::vector xnew; 180 | xnew.push_back( trans.x);xnew.push_back( trans.y); 181 | V.push_back(xnew); 182 | 183 | points.points.clear(); 184 | pub.publish(points) ; 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | std::vector frontiers; 193 | int i=0; 194 | float xr,yr; 195 | std::vector x_rand,x_nearest,x_new; 196 | 197 | 198 | // Main loop 199 | while (ros::ok()){ 200 | 201 | 202 | // Sample free 203 | x_rand.clear(); 204 | xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx; 205 | yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty; 206 | 207 | 208 | x_rand.push_back( xr ); x_rand.push_back( yr ); 209 | 210 | 211 | // Nearest 212 | x_nearest=Nearest(V,x_rand); 213 | 214 | // Steer 215 | 216 | x_new=Steer(x_nearest,x_rand,eta); 217 | 218 | 219 | // ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle 220 | char checking=ObstacleFree(x_nearest,x_new,mapData); 221 | 222 | if (checking==-1){ 223 | //geometry_msgs::PointStamped exploration_point; 224 | 225 | // the first point of a frontier 226 | p.x=x_new[0]; 227 | p.y=x_new[1]; 228 | p.z=0.0; 229 | 230 | ROS_DEBUG("before(%f, %f)", p.x, p.y); 231 | bool existFrontier = getCompleteFrontier(p, exploration_goal, mapData); 232 | if (!existFrontier) { 233 | continue; 234 | } 235 | 236 | exploration_goal.header.stamp=ros::Time(0); 237 | exploration_goal.header.frame_id=mapData.header.frame_id; 238 | //exploration_goal.point.x=x_new[0]; 239 | //exploration_goal.point.y=x_new[1]; 240 | exploration_goal.point.z=0.0; 241 | 242 | //p.x = exploration_goal.point.x; 243 | //p.y = exploration_goal.point.y; 244 | points.points.push_back(p); 245 | 246 | ROS_DEBUG("after(%f, %f)", p.x, p.y); 247 | 248 | pub.publish(points) ; 249 | targetspub.publish(exploration_goal); 250 | points.points.clear(); 251 | 252 | } 253 | 254 | 255 | else if (checking==1){ 256 | V.push_back(x_new); 257 | 258 | p.x=x_new[0]; 259 | p.y=x_new[1]; 260 | p.z=0.0; 261 | line.points.push_back(p); 262 | p.x=x_nearest[0]; 263 | p.y=x_nearest[1]; 264 | p.z=0.0; 265 | line.points.push_back(p); 266 | 267 | } 268 | 269 | 270 | 271 | pub.publish(line); 272 | 273 | 274 | 275 | 276 | ros::spinOnce(); 277 | rate.sleep(); 278 | }return 0;} 279 | -------------------------------------------------------------------------------- /include/mtrand.h: -------------------------------------------------------------------------------- 1 | // mtrand.h 2 | // C++ include file for MT19937, with initialization improved 2002/1/26. 3 | // Coded by Takuji Nishimura and Makoto Matsumoto. 4 | // Ported to C++ by Jasper Bedaux 2003/1/1 (see http://www.bedaux.net/mtrand/). 5 | // The generators returning floating point numbers are based on 6 | // a version by Isaku Wada, 2002/01/09 7 | // 8 | // Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, 9 | // All rights reserved. 10 | // 11 | // Redistribution and use in source and binary forms, with or without 12 | // modification, are permitted provided that the following conditions 13 | // are met: 14 | // 15 | // 1. Redistributions of source code must retain the above copyright 16 | // notice, this list of conditions and the following disclaimer. 17 | // 18 | // 2. Redistributions in binary form must reproduce the above copyright 19 | // notice, this list of conditions and the following disclaimer in the 20 | // documentation and/or other materials provided with the distribution. 21 | // 22 | // 3. The names of its contributors may not be used to endorse or promote 23 | // products derived from this software without specific prior written 24 | // permission. 25 | // 26 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 29 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 30 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 31 | // EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 32 | // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 33 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 34 | // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 35 | // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 36 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 37 | // 38 | // Any feedback is very welcome. 39 | // http://www.math.keio.ac.jp/matumoto/emt.html 40 | // email: matumoto@math.keio.ac.jp 41 | // 42 | // Feedback about the C++ port should be sent to Jasper Bedaux, 43 | // see http://www.bedaux.net/mtrand/ for e-mail address and info. 44 | 45 | #ifndef MTRAND_H 46 | #define MTRAND_H 47 | 48 | class MTRand_int32 { // Mersenne Twister random number generator 49 | public: 50 | // default constructor: uses default seed only if this is the first instance 51 | MTRand_int32() { if (!init) seed(5489UL); init = true; } 52 | // constructor with 32 bit int as seed 53 | MTRand_int32(unsigned long s) { seed(s); init = true; } 54 | // constructor with array of size 32 bit ints as seed 55 | MTRand_int32(const unsigned long* array, int size) { seed(array, size); init = true; } 56 | // the two seed functions 57 | void seed(unsigned long); // seed with 32 bit integer 58 | void seed(const unsigned long*, int size); // seed with array 59 | // overload operator() to make this a generator (functor) 60 | unsigned long operator()() { return rand_int32(); } 61 | // 2007-02-11: made the destructor virtual; thanks "double more" for pointing this out 62 | virtual ~MTRand_int32() {} // destructor 63 | protected: // used by derived classes, otherwise not accessible; use the ()-operator 64 | unsigned long rand_int32(); // generate 32 bit random integer 65 | private: 66 | static const int n = 624, m = 397; // compile time constants 67 | // the variables below are static (no duplicates can exist) 68 | static unsigned long state[n]; // state vector array 69 | static int p; // position in state array 70 | static bool init; // true if init function is called 71 | // private functions used to generate the pseudo random numbers 72 | unsigned long twiddle(unsigned long, unsigned long); // used by gen_state() 73 | void gen_state(); // generate new state 74 | // make copy constructor and assignment operator unavailable, they don't make sense 75 | MTRand_int32(const MTRand_int32&); // copy constructor not defined 76 | void operator=(const MTRand_int32&); // assignment operator not defined 77 | }; 78 | 79 | // inline for speed, must therefore reside in header file 80 | inline unsigned long MTRand_int32::twiddle(unsigned long u, unsigned long v) { 81 | return (((u & 0x80000000UL) | (v & 0x7FFFFFFFUL)) >> 1) 82 | ^ ((v & 1UL) * 0x9908B0DFUL); 83 | // 2013-07-22: line above modified for performance according to http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/Ierymenko.html 84 | // thanks Vitaliy FEOKTISTOV for pointing this out 85 | } 86 | 87 | inline unsigned long MTRand_int32::rand_int32() { // generate 32 bit random int 88 | if (p == n) gen_state(); // new state vector needed 89 | // gen_state() is split off to be non-inline, because it is only called once 90 | // in every 624 calls and otherwise irand() would become too big to get inlined 91 | unsigned long x = state[p++]; 92 | x ^= (x >> 11); 93 | x ^= (x << 7) & 0x9D2C5680UL; 94 | x ^= (x << 15) & 0xEFC60000UL; 95 | return x ^ (x >> 18); 96 | } 97 | 98 | // generates double floating point numbers in the half-open interval [0, 1) 99 | class MTRand : public MTRand_int32 { 100 | public: 101 | MTRand() : MTRand_int32() {} 102 | MTRand(unsigned long seed) : MTRand_int32(seed) {} 103 | MTRand(const unsigned long* seed, int size) : MTRand_int32(seed, size) {} 104 | ~MTRand() {} 105 | double operator()() { 106 | return static_cast(rand_int32()) * (1. / 4294967296.); } // divided by 2^32 107 | private: 108 | MTRand(const MTRand&); // copy constructor not defined 109 | void operator=(const MTRand&); // assignment operator not defined 110 | }; 111 | 112 | // generates double floating point numbers in the closed interval [0, 1] 113 | class MTRand_closed : public MTRand_int32 { 114 | public: 115 | MTRand_closed() : MTRand_int32() {} 116 | MTRand_closed(unsigned long seed) : MTRand_int32(seed) {} 117 | MTRand_closed(const unsigned long* seed, int size) : MTRand_int32(seed, size) {} 118 | ~MTRand_closed() {} 119 | double operator()() { 120 | return static_cast(rand_int32()) * (1. / 4294967295.); } // divided by 2^32 - 1 121 | private: 122 | MTRand_closed(const MTRand_closed&); // copy constructor not defined 123 | void operator=(const MTRand_closed&); // assignment operator not defined 124 | }; 125 | 126 | // generates double floating point numbers in the open interval (0, 1) 127 | class MTRand_open : public MTRand_int32 { 128 | public: 129 | MTRand_open() : MTRand_int32() {} 130 | MTRand_open(unsigned long seed) : MTRand_int32(seed) {} 131 | MTRand_open(const unsigned long* seed, int size) : MTRand_int32(seed, size) {} 132 | ~MTRand_open() {} 133 | double operator()() { 134 | return (static_cast(rand_int32()) + .5) * (1. / 4294967296.); } // divided by 2^32 135 | private: 136 | MTRand_open(const MTRand_open&); // copy constructor not defined 137 | void operator=(const MTRand_open&); // assignment operator not defined 138 | }; 139 | 140 | // generates 53 bit resolution doubles in the half-open interval [0, 1) 141 | class MTRand53 : public MTRand_int32 { 142 | public: 143 | MTRand53() : MTRand_int32() {} 144 | MTRand53(unsigned long seed) : MTRand_int32(seed) {} 145 | MTRand53(const unsigned long* seed, int size) : MTRand_int32(seed, size) {} 146 | ~MTRand53() {} 147 | double operator()() { 148 | return (static_cast(rand_int32() >> 5) * 67108864. + 149 | static_cast(rand_int32() >> 6)) * (1. / 9007199254740992.); } 150 | private: 151 | MTRand53(const MTRand53&); // copy constructor not defined 152 | void operator=(const MTRand53&); // assignment operator not defined 153 | }; 154 | 155 | #endif // MTRAND_H 156 | -------------------------------------------------------------------------------- /src/local_rrt_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "stdint.h" 8 | #include "functions.h" 9 | #include "mtrand.h" 10 | 11 | 12 | #include "nav_msgs/OccupancyGrid.h" 13 | #include "geometry_msgs/PointStamped.h" 14 | #include "std_msgs/Header.h" 15 | #include "nav_msgs/MapMetaData.h" 16 | #include "geometry_msgs/Point.h" 17 | #include "visualization_msgs/Marker.h" 18 | #include 19 | 20 | 21 | 22 | // global variables 23 | nav_msgs::OccupancyGrid mapData; 24 | geometry_msgs::PointStamped clickedpoint; 25 | geometry_msgs::PointStamped exploration_goal; 26 | visualization_msgs::Marker points,line; 27 | float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y; 28 | 29 | rdm r; // for genrating random numbers 30 | 31 | 32 | 33 | //Subscribers callback functions--------------------------------------- 34 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 35 | { 36 | mapData=*msg; 37 | } 38 | 39 | 40 | 41 | void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg) 42 | { 43 | 44 | geometry_msgs::Point p; 45 | p.x=msg->point.x; 46 | p.y=msg->point.y; 47 | p.z=msg->point.z; 48 | 49 | points.points.push_back(p); 50 | 51 | } 52 | 53 | 54 | 55 | 56 | int main(int argc, char **argv) 57 | { 58 | 59 | unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7; 60 | MTRand_int32 irand(init, length); // 32-bit int generator 61 | // this is an example of initializing by an array 62 | // you may use MTRand(seed) with any 32bit integer 63 | // as a seed for a simpler initialization 64 | MTRand drand; // double in [0, 1) generator, already init 65 | 66 | // generate the same numbers as in the original C test program 67 | ros::init(argc, argv, "local_rrt_frontier_detector"); 68 | ros::NodeHandle nh; 69 | 70 | // fetching all parameters 71 | float eta,init_map_x,init_map_y,range; 72 | std::string map_topic,base_frame_topic; 73 | 74 | std::string ns; 75 | ns=ros::this_node::getName(); 76 | 77 | ros::param::param(ns+"/eta", eta, 0.5); 78 | ros::param::param(ns+"/map_topic", map_topic, "robot_1/map"); 79 | ros::param::param(ns+"/robot_frame", base_frame_topic, "/robot_1/base_footprint"); 80 | //--------------------------------------------------------------- 81 | ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack); 82 | ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack); 83 | 84 | ros::Publisher targetspub = nh.advertise("/detected_points", 10); 85 | ros::Publisher pub = nh.advertise(ns+"_shapes", 10); 86 | 87 | ros::Rate rate(100); 88 | 89 | 90 | // wait until map is received, when a map is received, mapData.header.seq will not be < 1 91 | while (mapData.header.seq<1 or mapData.data.size()<1) { ros::spinOnce(); ros::Duration(0.1).sleep();} 92 | 93 | 94 | 95 | //visualizations points and lines.. 96 | points.header.frame_id=mapData.header.frame_id; 97 | line.header.frame_id=mapData.header.frame_id; 98 | points.header.stamp=ros::Time(0); 99 | line.header.stamp=ros::Time(0); 100 | 101 | points.ns=line.ns = "markers"; 102 | points.id = 0; 103 | line.id =1; 104 | 105 | 106 | points.type = points.POINTS; 107 | line.type=line.LINE_LIST; 108 | 109 | //Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 110 | points.action =points.ADD; 111 | line.action = line.ADD; 112 | points.pose.orientation.w =1.0; 113 | line.pose.orientation.w = 1.0; 114 | line.scale.x = 0.03; 115 | line.scale.y= 0.03; 116 | points.scale.x=0.3; 117 | points.scale.y=0.3; 118 | 119 | line.color.r =255.0/255.0; 120 | line.color.g= 0.0/255.0; 121 | line.color.b =0.0/255.0; 122 | points.color.r = 255.0/255.0; 123 | points.color.g = 0.0/255.0; 124 | points.color.b = 0.0/255.0; 125 | points.color.a=0.3; 126 | line.color.a = 1.0; 127 | points.lifetime = ros::Duration(); 128 | line.lifetime = ros::Duration(); 129 | 130 | geometry_msgs::Point p; 131 | 132 | 133 | while(points.points.size()<5) 134 | { 135 | ros::spinOnce(); 136 | 137 | pub.publish(points) ; 138 | } 139 | 140 | 141 | 142 | 143 | std::vector temp1; 144 | temp1.push_back(points.points[0].x); 145 | temp1.push_back(points.points[0].y); 146 | 147 | std::vector temp2; 148 | temp2.push_back(points.points[2].x); 149 | temp2.push_back(points.points[0].y); 150 | 151 | 152 | init_map_x=Norm(temp1,temp2); 153 | temp1.clear(); temp2.clear(); 154 | 155 | temp1.push_back(points.points[0].x); 156 | temp1.push_back(points.points[0].y); 157 | 158 | temp2.push_back(points.points[0].x); 159 | temp2.push_back(points.points[2].y); 160 | 161 | init_map_y=Norm(temp1,temp2); 162 | temp1.clear(); temp2.clear(); 163 | 164 | Xstartx=(points.points[0].x+points.points[2].x)*.5; 165 | Xstarty=(points.points[0].y+points.points[2].y)*.5; 166 | 167 | 168 | 169 | 170 | 171 | geometry_msgs::Point trans; 172 | trans=points.points[4]; 173 | std::vector< std::vector > V; 174 | std::vector xnew; 175 | xnew.push_back( trans.x);xnew.push_back( trans.y); 176 | V.push_back(xnew); 177 | 178 | points.points.clear(); 179 | pub.publish(points) ; 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | std::vector frontiers; 188 | int i=0; 189 | float xr,yr; 190 | std::vector x_rand,x_nearest,x_new; 191 | 192 | tf::TransformListener listener; 193 | // Main loop 194 | while (ros::ok()){ 195 | 196 | 197 | // Sample free 198 | x_rand.clear(); 199 | xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx; 200 | yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty; 201 | 202 | 203 | x_rand.push_back( xr ); x_rand.push_back( yr ); 204 | 205 | 206 | // Nearest 207 | x_nearest=Nearest(V,x_rand); 208 | 209 | // Steer 210 | 211 | x_new=Steer(x_nearest,x_rand,eta); 212 | 213 | 214 | // ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle 215 | char checking=ObstacleFree(x_nearest,x_new,mapData); 216 | 217 | if (checking==-1){ 218 | 219 | p.x=x_new[0]; 220 | p.y=x_new[1]; 221 | p.z=0.0; 222 | 223 | bool existFrontier = getCompleteFrontier(p, exploration_goal, mapData); 224 | if (!existFrontier) { 225 | continue; 226 | } 227 | exploration_goal.header.stamp=ros::Time(0); 228 | exploration_goal.header.frame_id=mapData.header.frame_id; 229 | 230 | //exploration_goal.point.x=x_new[0]; 231 | //exploration_goal.point.y=x_new[1]; 232 | exploration_goal.point.z=0.0; 233 | 234 | //p.x = exploration_goal.point.x; 235 | //p.y = exploration_goal.point.y; 236 | 237 | points.points.push_back(p); 238 | pub.publish(points); 239 | targetspub.publish(exploration_goal); 240 | points.points.clear(); 241 | V.clear(); 242 | 243 | 244 | tf::StampedTransform transform; 245 | int temp=0; 246 | while (temp==0){ 247 | try{ 248 | temp=1; 249 | listener.lookupTransform(map_topic, base_frame_topic , ros::Time(0), transform); 250 | } 251 | catch (tf::TransformException ex){ 252 | temp=0; 253 | ros::Duration(0.1).sleep(); 254 | }} 255 | 256 | x_new[0]=transform.getOrigin().x(); 257 | x_new[1]=transform.getOrigin().y(); 258 | V.push_back(x_new); 259 | line.points.clear(); 260 | } 261 | 262 | 263 | else if (checking==1){ 264 | V.push_back(x_new); 265 | 266 | p.x=x_new[0]; 267 | p.y=x_new[1]; 268 | p.z=0.0; 269 | line.points.push_back(p); 270 | p.x=x_nearest[0]; 271 | p.y=x_nearest[1]; 272 | p.z=0.0; 273 | line.points.push_back(p); 274 | 275 | } 276 | 277 | 278 | 279 | pub.publish(line); 280 | 281 | 282 | 283 | 284 | ros::spinOnce(); 285 | rate.sleep(); 286 | }return 0;} 287 | -------------------------------------------------------------------------------- /src/local_rrt_detector_real_two_robots.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "stdint.h" 8 | #include "functions.h" 9 | #include "mtrand.h" 10 | 11 | 12 | #include "nav_msgs/OccupancyGrid.h" 13 | #include "geometry_msgs/PointStamped.h" 14 | #include "std_msgs/Header.h" 15 | #include "nav_msgs/MapMetaData.h" 16 | #include "geometry_msgs/Point.h" 17 | #include "visualization_msgs/Marker.h" 18 | #include 19 | 20 | 21 | 22 | // global variables 23 | nav_msgs::OccupancyGrid mapData; 24 | geometry_msgs::PointStamped clickedpoint; 25 | geometry_msgs::PointStamped exploration_goal; 26 | visualization_msgs::Marker points,line; 27 | float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y; 28 | 29 | rdm r; // for genrating random numbers 30 | 31 | 32 | 33 | //Subscribers callback functions--------------------------------------- 34 | void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) 35 | { 36 | mapData=*msg; 37 | } 38 | 39 | 40 | 41 | void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg) 42 | { 43 | 44 | geometry_msgs::Point p; 45 | p.x=msg->point.x; 46 | p.y=msg->point.y; 47 | p.z=msg->point.z; 48 | 49 | points.points.push_back(p); 50 | 51 | } 52 | 53 | 54 | 55 | 56 | int main(int argc, char **argv) 57 | { 58 | 59 | unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7; 60 | MTRand_int32 irand(init, length); // 32-bit int generator 61 | // this is an example of initializing by an array 62 | // you may use MTRand(seed) with any 32bit integer 63 | // as a seed for a simpler initialization 64 | MTRand drand; // double in [0, 1) generator, already init 65 | 66 | // generate the same numbers as in the original C test program 67 | ros::init(argc, argv, "local_rrt_frontier_detector"); 68 | ros::NodeHandle nh; 69 | 70 | // fetching all parameters 71 | float eta,init_map_x,init_map_y,range; 72 | std::string map_topic,base_frame_topic; 73 | 74 | std::string ns; 75 | ns=ros::this_node::getName(); 76 | 77 | ros::param::param(ns+"/eta", eta, 0.5); 78 | ros::param::param(ns+"/map_topic", map_topic, "/robot_1/map"); 79 | ros::param::param(ns+"/robot_frame", base_frame_topic, "/robot_1/base_link"); 80 | //--------------------------------------------------------------- 81 | ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack); 82 | ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack); 83 | 84 | ros::Publisher targetspub = nh.advertise("/detected_points", 10); 85 | ros::Publisher pub = nh.advertise(ns+"_shapes", 10); 86 | 87 | ros::Rate rate(100); 88 | 89 | 90 | // wait until map is received, when a map is received, mapData.header.seq will not be < 1 91 | while (mapData.header.seq<1 or mapData.data.size()<1) { ros::spinOnce(); ros::Duration(0.1).sleep();} 92 | 93 | 94 | 95 | //visualizations points and lines.. 96 | points.header.frame_id=mapData.header.frame_id; 97 | line.header.frame_id=mapData.header.frame_id; 98 | points.header.stamp=ros::Time(0); 99 | line.header.stamp=ros::Time(0); 100 | 101 | points.ns=line.ns = "markers"; 102 | points.id = 0; 103 | line.id =1; 104 | 105 | 106 | points.type = points.POINTS; 107 | line.type=line.LINE_LIST; 108 | 109 | //Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 110 | points.action =points.ADD; 111 | line.action = line.ADD; 112 | points.pose.orientation.w =1.0; 113 | line.pose.orientation.w = 1.0; 114 | line.scale.x = 0.03; 115 | line.scale.y= 0.03; 116 | points.scale.x=0.3; 117 | points.scale.y=0.3; 118 | 119 | line.color.r =255.0/255.0; 120 | line.color.g= 0.0/255.0; 121 | line.color.b =0.0/255.0; 122 | points.color.r = 255.0/255.0; 123 | points.color.g = 0.0/255.0; 124 | points.color.b = 0.0/255.0; 125 | points.color.a=0.3; 126 | line.color.a = 1.0; 127 | points.lifetime = ros::Duration(); 128 | line.lifetime = ros::Duration(); 129 | 130 | geometry_msgs::Point p; 131 | 132 | 133 | while(points.points.size()<5) 134 | { 135 | ros::spinOnce(); 136 | 137 | pub.publish(points) ; 138 | } 139 | 140 | 141 | 142 | 143 | std::vector temp1; 144 | temp1.push_back(points.points[0].x); 145 | temp1.push_back(points.points[0].y); 146 | 147 | std::vector temp2; 148 | temp2.push_back(points.points[2].x); 149 | temp2.push_back(points.points[0].y); 150 | 151 | 152 | init_map_x=Norm(temp1,temp2); 153 | temp1.clear(); temp2.clear(); 154 | 155 | temp1.push_back(points.points[0].x); 156 | temp1.push_back(points.points[0].y); 157 | 158 | temp2.push_back(points.points[0].x); 159 | temp2.push_back(points.points[2].y); 160 | 161 | init_map_y=Norm(temp1,temp2); 162 | temp1.clear(); temp2.clear(); 163 | 164 | Xstartx=(points.points[0].x+points.points[2].x)*.5; 165 | Xstarty=(points.points[0].y+points.points[2].y)*.5; 166 | 167 | 168 | 169 | 170 | 171 | geometry_msgs::Point trans; 172 | trans=points.points[4]; 173 | std::vector< std::vector > V; 174 | std::vector xnew; 175 | xnew.push_back( trans.x);xnew.push_back( trans.y); 176 | V.push_back(xnew); 177 | 178 | points.points.clear(); 179 | pub.publish(points) ; 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | std::vector frontiers; 188 | int i=0; 189 | float xr,yr; 190 | std::vector x_rand,x_nearest,x_new; 191 | 192 | tf::TransformListener listener; 193 | // Main loop 194 | while (ros::ok()){ 195 | 196 | 197 | // Sample free 198 | x_rand.clear(); 199 | xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx; 200 | yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty; 201 | 202 | 203 | x_rand.push_back( xr ); x_rand.push_back( yr ); 204 | 205 | 206 | // Nearest 207 | x_nearest=Nearest(V,x_rand); 208 | 209 | // Steer 210 | 211 | x_new=Steer(x_nearest,x_rand,eta); 212 | 213 | 214 | // ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle 215 | char checking=ObstacleFree(x_nearest,x_new,mapData); 216 | 217 | if (checking==-1){ 218 | 219 | p.x=x_new[0]; 220 | p.y=x_new[1]; 221 | p.z=0.0; 222 | 223 | bool existFrontier = getCompleteFrontier(p, exploration_goal, mapData); 224 | if (!existFrontier) { 225 | continue; 226 | } 227 | exploration_goal.header.stamp=ros::Time(0); 228 | exploration_goal.header.frame_id=mapData.header.frame_id; 229 | 230 | //exploration_goal.point.x=x_new[0]; 231 | //exploration_goal.point.y=x_new[1]; 232 | exploration_goal.point.z=0.0; 233 | 234 | //p.x = exploration_goal.point.x; 235 | //p.y = exploration_goal.point.y; 236 | 237 | points.points.push_back(p); 238 | pub.publish(points); 239 | targetspub.publish(exploration_goal); 240 | points.points.clear(); 241 | V.clear(); 242 | 243 | 244 | tf::StampedTransform transform; 245 | int temp=0; 246 | while (temp==0){ 247 | try{ 248 | temp=1; 249 | listener.lookupTransform(map_topic, base_frame_topic , ros::Time(0), transform); 250 | } 251 | catch (tf::TransformException ex){ 252 | temp=0; 253 | ros::Duration(0.1).sleep(); 254 | }} 255 | 256 | x_new[0]=transform.getOrigin().x(); 257 | x_new[1]=transform.getOrigin().y(); 258 | V.push_back(x_new); 259 | line.points.clear(); 260 | } 261 | 262 | 263 | else if (checking==1){ 264 | V.push_back(x_new); 265 | 266 | p.x=x_new[0]; 267 | p.y=x_new[1]; 268 | p.z=0.0; 269 | line.points.push_back(p); 270 | p.x=x_nearest[0]; 271 | p.y=x_nearest[1]; 272 | p.z=0.0; 273 | line.points.push_back(p); 274 | 275 | } 276 | 277 | 278 | 279 | pub.publish(line); 280 | 281 | 282 | 283 | 284 | ros::spinOnce(); 285 | rate.sleep(); 286 | }return 0;} 287 | -------------------------------------------------------------------------------- /scripts/filter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #--------Include modules--------------- 4 | from copy import copy 5 | import rospy 6 | from visualization_msgs.msg import Marker 7 | from geometry_msgs.msg import Point 8 | from nav_msgs.msg import OccupancyGrid 9 | from geometry_msgs.msg import PointStamped 10 | import tf 11 | from numpy import array,vstack,delete 12 | from functions import gridValue,informationGain 13 | from sklearn.cluster import MeanShift 14 | from multi_rrt_exploration.msg import PointArray 15 | 16 | # Subscribers' callbacks------------------------------ 17 | mapData=OccupancyGrid() 18 | frontiers=[] 19 | globalmaps=[] 20 | def callBack(data,args): 21 | global frontiers,min_distance 22 | transformedPoint=args[0].transformPoint(args[1],data) 23 | x=[array([transformedPoint.point.x,transformedPoint.point.y])] 24 | if len(frontiers)>0: 25 | frontiers=vstack((frontiers,x)) 26 | else: 27 | frontiers=x 28 | 29 | 30 | def mapCallBack(data): 31 | global mapData 32 | mapData=data 33 | 34 | def globalMap(data): 35 | global global1,globalmaps,litraIndx,namespace_init_count,n_robots 36 | global1=data 37 | if n_robots>1: 38 | indx=int(data._connection_header['topic'][litraIndx])-namespace_init_count 39 | elif n_robots==1: 40 | indx=0 41 | globalmaps[indx]=data 42 | 43 | # Node---------------------------------------------- 44 | def node(): 45 | global frontiers,mapData,global1,global2,global3,globalmaps,litraIndx,n_robots,namespace_init_count 46 | rospy.init_node('filter', anonymous=False) 47 | 48 | # fetching all parameters 49 | map_topic= rospy.get_param('~map_topic','/map_merge/map') 50 | threshold= rospy.get_param('~costmap_clearing_threshold',70) 51 | info_radius= rospy.get_param('~info_radius',1.0) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate 52 | goals_topic= rospy.get_param('~goals_topic','/detected_points') 53 | n_robots = rospy.get_param('~n_robots',1) 54 | namespace = rospy.get_param('~namespace','') 55 | namespace_init_count = rospy.get_param('~namespace_init_count',1) 56 | rateHz = rospy.get_param('~rate',2) 57 | litraIndx=len(namespace) 58 | rate = rospy.Rate(rateHz) 59 | #------------------------------------------- 60 | rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) 61 | 62 | 63 | #--------------------------------------------------------------------------------------------------------------- 64 | 65 | 66 | for i in range(0,n_robots): 67 | globalmaps.append(OccupancyGrid()) 68 | 69 | if len(namespace) > 0: 70 | for i in range(0,n_robots): 71 | rospy.Subscriber(namespace+str(i+namespace_init_count)+'/move_base/global_costmap/costmap', OccupancyGrid, globalMap) 72 | elif len(namespace)==0: 73 | rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid, globalMap) 74 | #wait if map is not received yet 75 | while (len(mapData.data)<1): 76 | pass 77 | #wait if any of robots' global costmap map is not received yet 78 | for i in range(0,n_robots): 79 | while (len(globalmaps[i].data)<1): 80 | pass 81 | 82 | global_frame="/"+mapData.header.frame_id 83 | 84 | 85 | tfLisn=tf.TransformListener() 86 | if len(namespace) > 0: 87 | for i in range(0,n_robots): 88 | tfLisn.waitForTransform(global_frame[1:], namespace+str(i+namespace_init_count)+'/base_footprint', rospy.Time(0),rospy.Duration(10.0)) 89 | elif len(namespace)==0: 90 | tfLisn.waitForTransform(global_frame[1:], '/base_footprint', rospy.Time(0),rospy.Duration(10.0)) 91 | 92 | rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]]) 93 | pub = rospy.Publisher('frontiers', Marker, queue_size=10) 94 | pub2 = rospy.Publisher('centroids', Marker, queue_size=10) 95 | filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10) 96 | 97 | rospy.loginfo("the map and global costmaps are received") 98 | 99 | 100 | # wait if no frontier is received yet 101 | while len(frontiers)<1: 102 | pass 103 | 104 | 105 | points=Marker() 106 | points_clust=Marker() 107 | #Set the frame ID and timestamp. See the TF tutorials for information on these. 108 | points.header.frame_id= mapData.header.frame_id 109 | points.header.stamp= rospy.Time.now() 110 | 111 | points.ns= "markers2" 112 | points.id = 0 113 | 114 | points.type = Marker.POINTS 115 | 116 | #Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 117 | points.action = Marker.ADD; 118 | 119 | points.pose.orientation.w = 1.0 120 | 121 | points.scale.x=0.2 122 | points.scale.y=0.2 123 | 124 | points.color.r = 255.0/255.0 125 | points.color.g = 255.0/255.0 126 | points.color.b = 0.0/255.0 127 | 128 | points.color.a=1; 129 | points.lifetime = rospy.Duration(); 130 | 131 | p=Point() 132 | 133 | p.z = 0; 134 | 135 | pp=[] 136 | pl=[] 137 | 138 | points_clust.header.frame_id= mapData.header.frame_id 139 | points_clust.header.stamp= rospy.Time.now() 140 | 141 | points_clust.ns= "markers3" 142 | points_clust.id = 4 143 | 144 | points_clust.type = Marker.POINTS 145 | 146 | #Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 147 | points_clust.action = Marker.ADD; 148 | 149 | points_clust.pose.orientation.w = 1.0; 150 | 151 | points_clust.scale.x=0.2; 152 | points_clust.scale.y=0.2; 153 | points_clust.color.r = 0.0/255.0 154 | points_clust.color.g = 255.0/255.0 155 | points_clust.color.b = 0.0/255.0 156 | 157 | points_clust.color.a=1; 158 | points_clust.lifetime = rospy.Duration(); 159 | 160 | 161 | temppoint=PointStamped() 162 | temppoint.header.frame_id= mapData.header.frame_id 163 | temppoint.header.stamp=rospy.Time(0) 164 | temppoint.point.z=0.0 165 | 166 | arraypoints=PointArray() 167 | tempPoint=Point() 168 | tempPoint.z=0.0 169 | #------------------------------------------------------------------------- 170 | #--------------------- Main Loop ------------------------------- 171 | #------------------------------------------------------------------------- 172 | while not rospy.is_shutdown(): 173 | #------------------------------------------------------------------------- 174 | #Clustering frontier points 175 | centroids=[] 176 | front=copy(frontiers) 177 | if len(front)>1: 178 | ms = MeanShift(bandwidth=0.3) 179 | ms.fit(front) 180 | centroids= ms.cluster_centers_ #centroids array is the centers of each cluster 181 | 182 | #if there is only one frontier no need for clustering, i.e. centroids=frontiers 183 | if len(front)==1: 184 | centroids=front 185 | frontiers=copy(centroids) 186 | #------------------------------------------------------------------------- 187 | #clearing old frontiers 188 | 189 | z=0 190 | while zthreshold) or cond 201 | if (cond or (informationGain(mapData,[centroids[z][0],centroids[z][1]],info_radius*0.5))<0.2): 202 | centroids=delete(centroids, (z), axis=0) 203 | z=z-1 204 | z+=1 205 | #------------------------------------------------------------------------- 206 | #publishing 207 | arraypoints.points=[] 208 | for i in centroids: 209 | tempPoint.x=i[0] 210 | tempPoint.y=i[1] 211 | arraypoints.points.append(copy(tempPoint)) 212 | filterpub.publish(arraypoints) 213 | pp=[] 214 | for q in range(0,len(frontiers)): 215 | p.x=frontiers[q][0] 216 | p.y=frontiers[q][1] 217 | pp.append(copy(p)) 218 | points.points=pp 219 | 220 | pp=[] 221 | for q in range(0,len(centroids)): 222 | p.x=centroids[q][0] 223 | p.y=centroids[q][1] 224 | pp.append(copy(p)) 225 | points_clust.points=pp 226 | pub.publish(points) 227 | pub2.publish(points_clust) 228 | rate.sleep() 229 | #------------------------------------------------------------------------- 230 | 231 | if __name__ == '__main__': 232 | try: 233 | node() 234 | except rospy.ROSInterruptException: 235 | pass 236 | 237 | 238 | 239 | 240 | -------------------------------------------------------------------------------- /scripts/filter_real_two_robots.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #--------Include modules--------------- 4 | from copy import copy 5 | import rospy 6 | from visualization_msgs.msg import Marker 7 | from geometry_msgs.msg import Point 8 | from nav_msgs.msg import OccupancyGrid 9 | from geometry_msgs.msg import PointStamped 10 | import tf 11 | from numpy import array,vstack,delete 12 | from functions_real_two_robots import gridValue,informationGain 13 | from sklearn.cluster import MeanShift 14 | from rrt_exploration.msg import PointArray 15 | 16 | # Subscribers' callbacks------------------------------ 17 | mapData=OccupancyGrid() 18 | frontiers=[] 19 | globalmaps=[] 20 | def callBack(data,args): 21 | global frontiers,min_distance 22 | transformedPoint=args[0].transformPoint(args[1],data) 23 | x=[array([transformedPoint.point.x,transformedPoint.point.y])] 24 | if len(frontiers)>0: 25 | frontiers=vstack((frontiers,x)) 26 | else: 27 | frontiers=x 28 | 29 | 30 | def mapCallBack(data): 31 | global mapData 32 | mapData=data 33 | 34 | def globalMap(data): 35 | global global1,globalmaps,litraIndx,namespace_init_count,n_robots 36 | global1=data 37 | if n_robots>1: 38 | indx=int(data._connection_header['topic'][litraIndx])-namespace_init_count 39 | elif n_robots==1: 40 | indx=0 41 | globalmaps[indx]=data 42 | 43 | # Node---------------------------------------------- 44 | def node(): 45 | global frontiers,mapData,global1,global2,global3,globalmaps,litraIndx,n_robots,namespace_init_count 46 | rospy.init_node('filter', anonymous=False) 47 | 48 | # fetching all parameters 49 | map_topic= rospy.get_param('~map_topic','/map') 50 | threshold= rospy.get_param('~costmap_clearing_threshold',70) 51 | info_radius= rospy.get_param('~info_radius',1.0) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate 52 | goals_topic= rospy.get_param('~goals_topic','/detected_points') 53 | n_robots = rospy.get_param('~n_robots',1) 54 | namespace = rospy.get_param('~namespace','') 55 | namespace_init_count = rospy.get_param('~namespace_init_count',1) 56 | rateHz = rospy.get_param('~rate',2) 57 | litraIndx=len(namespace) 58 | rate = rospy.Rate(rateHz) 59 | #------------------------------------------- 60 | rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) 61 | 62 | 63 | #--------------------------------------------------------------------------------------------------------------- 64 | 65 | 66 | for i in range(0,n_robots): 67 | globalmaps.append(OccupancyGrid()) 68 | 69 | if len(namespace) > 0: 70 | for i in range(0,n_robots): 71 | rospy.Subscriber(namespace+str(i+namespace_init_count)+'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) 72 | elif len(namespace)==0: 73 | rospy.Subscriber('/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) 74 | #wait if map is not received yet 75 | while (len(mapData.data)<1): 76 | pass 77 | #wait if any of robots' global costmap map is not received yet 78 | for i in range(0,n_robots): 79 | while (len(globalmaps[i].data)<1): 80 | pass 81 | 82 | global_frame="/"+mapData.header.frame_id 83 | 84 | 85 | tfLisn=tf.TransformListener() 86 | if len(namespace) > 0: 87 | for i in range(0,n_robots): 88 | tfLisn.waitForTransform(global_frame[1:], namespace+str(i+namespace_init_count)+'/base_footprint', rospy.Time(0),rospy.Duration(10.0)) 89 | elif len(namespace)==0: 90 | tfLisn.waitForTransform(global_frame[1:], '/base_footprint', rospy.Time(0),rospy.Duration(10.0)) 91 | 92 | rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]]) 93 | pub = rospy.Publisher('frontiers', Marker, queue_size=10) 94 | pub2 = rospy.Publisher('centroids', Marker, queue_size=10) 95 | filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10) 96 | 97 | rospy.loginfo("the map and global costmaps are received") 98 | 99 | 100 | # wait if no frontier is received yet 101 | while len(frontiers)<1: 102 | pass 103 | 104 | 105 | points=Marker() 106 | points_clust=Marker() 107 | #Set the frame ID and timestamp. See the TF tutorials for information on these. 108 | points.header.frame_id= mapData.header.frame_id 109 | points.header.stamp= rospy.Time.now() 110 | 111 | points.ns= "markers2" 112 | points.id = 0 113 | 114 | points.type = Marker.POINTS 115 | 116 | #Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 117 | points.action = Marker.ADD; 118 | 119 | points.pose.orientation.w = 1.0 120 | 121 | points.scale.x=0.2 122 | points.scale.y=0.2 123 | 124 | points.color.r = 255.0/255.0 125 | points.color.g = 255.0/255.0 126 | points.color.b = 0.0/255.0 127 | 128 | points.color.a=1; 129 | points.lifetime = rospy.Duration(); 130 | 131 | p=Point() 132 | 133 | p.z = 0; 134 | 135 | pp=[] 136 | pl=[] 137 | 138 | points_clust.header.frame_id= mapData.header.frame_id 139 | points_clust.header.stamp= rospy.Time.now() 140 | 141 | points_clust.ns= "markers3" 142 | points_clust.id = 4 143 | 144 | points_clust.type = Marker.POINTS 145 | 146 | #Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 147 | points_clust.action = Marker.ADD; 148 | 149 | points_clust.pose.orientation.w = 1.0; 150 | 151 | points_clust.scale.x=0.2; 152 | points_clust.scale.y=0.2; 153 | points_clust.color.r = 0.0/255.0 154 | points_clust.color.g = 255.0/255.0 155 | points_clust.color.b = 0.0/255.0 156 | 157 | points_clust.color.a=1; 158 | points_clust.lifetime = rospy.Duration(); 159 | 160 | 161 | temppoint=PointStamped() 162 | temppoint.header.frame_id= mapData.header.frame_id 163 | temppoint.header.stamp=rospy.Time(0) 164 | temppoint.point.z=0.0 165 | 166 | arraypoints=PointArray() 167 | tempPoint=Point() 168 | tempPoint.z=0.0 169 | #------------------------------------------------------------------------- 170 | #--------------------- Main Loop ------------------------------- 171 | #------------------------------------------------------------------------- 172 | while not rospy.is_shutdown(): 173 | #------------------------------------------------------------------------- 174 | #Clustering frontier points 175 | centroids=[] 176 | front=copy(frontiers) 177 | if len(front)>1: 178 | ms = MeanShift(bandwidth=0.3) 179 | ms.fit(front) 180 | centroids= ms.cluster_centers_ #centroids array is the centers of each cluster 181 | 182 | #if there is only one frontier no need for clustering, i.e. centroids=frontiers 183 | if len(front)==1: 184 | centroids=front 185 | frontiers=copy(centroids) 186 | #------------------------------------------------------------------------- 187 | #clearing old frontiers 188 | 189 | z=0 190 | while zthreshold) or cond 201 | if (cond or (informationGain(mapData,[centroids[z][0],centroids[z][1]],info_radius*0.5))<0.2): 202 | centroids=delete(centroids, (z), axis=0) 203 | z=z-1 204 | z+=1 205 | #------------------------------------------------------------------------- 206 | #publishing 207 | arraypoints.points=[] 208 | for i in centroids: 209 | tempPoint.x=i[0] 210 | tempPoint.y=i[1] 211 | arraypoints.points.append(copy(tempPoint)) 212 | filterpub.publish(arraypoints) 213 | pp=[] 214 | for q in range(0,len(frontiers)): 215 | p.x=frontiers[q][0] 216 | p.y=frontiers[q][1] 217 | pp.append(copy(p)) 218 | points.points=pp 219 | 220 | pp=[] 221 | for q in range(0,len(centroids)): 222 | p.x=centroids[q][0] 223 | p.y=centroids[q][1] 224 | pp.append(copy(p)) 225 | points_clust.points=pp 226 | pub.publish(points) 227 | pub2.publish(points_clust) 228 | rate.sleep() 229 | #------------------------------------------------------------------------- 230 | 231 | if __name__ == '__main__': 232 | try: 233 | node() 234 | except rospy.ROSInterruptException: 235 | pass 236 | 237 | 238 | 239 | 240 | -------------------------------------------------------------------------------- /scripts/filter_backup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #--------Include modules--------------- 4 | from copy import copy 5 | import rospy 6 | from visualization_msgs.msg import Marker 7 | from geometry_msgs.msg import Point 8 | from nav_msgs.msg import OccupancyGrid 9 | from geometry_msgs.msg import PointStamped 10 | import tf 11 | from numpy import array,vstack,delete 12 | from functions import gridValue,informationGain2,isOldFrontier 13 | from sklearn.cluster import MeanShift 14 | from rrt_exploration.msg import PointArray 15 | 16 | # Subscribers' callbacks------------------------------ 17 | mapData=OccupancyGrid() 18 | frontiers=set() 19 | globalmaps=[] 20 | def callBack(data,args): 21 | global frontiers,min_distance 22 | 23 | # transform from local map point to global map 24 | transformedPoint=args[0].transformPoint(args[1],data) 25 | x=(transformedPoint.point.x,transformedPoint.point.y) 26 | frontiers.add(copy(x)) 27 | #if len(frontiers)>0: 28 | # frontiers=vstack((frontiers,x)) 29 | #else: 30 | # frontiers=x 31 | 32 | 33 | def mapCallBack(data): 34 | global mapData 35 | mapData=data 36 | 37 | def globalMap(data): 38 | global global1,globalmaps,litraIndx,namespace_init_count,n_robots 39 | global1=data 40 | if n_robots>1: 41 | indx=int(data._connection_header['topic'][litraIndx])-namespace_init_count 42 | elif n_robots==1: 43 | indx=0 44 | globalmaps[indx]=data 45 | 46 | # Node---------------------------------------------- 47 | def node(): 48 | global frontiers,mapData,global1,global2,global3,globalmaps,litraIndx,n_robots,namespace_init_count 49 | rospy.init_node('filter', anonymous=False) 50 | 51 | # fetching all parameters 52 | map_topic= rospy.get_param('~map_topic','/map') 53 | threshold= rospy.get_param('~costmap_clearing_threshold',70) 54 | info_radius= rospy.get_param('~info_radius',1.0) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate 55 | goals_topic= rospy.get_param('~goals_topic','/detected_points') 56 | n_robots = rospy.get_param('~n_robots',1) 57 | namespace = rospy.get_param('~namespace','') 58 | namespace_init_count = rospy.get_param('namespace_init_count',1) 59 | rateHz = rospy.get_param('~rate',1) 60 | litraIndx=len(namespace) 61 | rate = rospy.Rate(rateHz) 62 | #--------------------------------------- 63 | rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) 64 | 65 | 66 | #--------------------------------------------------------------------------------------------------------------- 67 | for i in range(0,n_robots): 68 | globalmaps.append(OccupancyGrid()) 69 | 70 | if len(namespace) > 0: 71 | for i in range(0,n_robots): 72 | rospy.Subscriber(namespace+str(i+namespace_init_count)+'/move_base/global_costmap/costmap', OccupancyGrid, globalMap) 73 | elif len(namespace)==0: 74 | rospy.Subscriber('/move_base/global_costmap/costmap', OccupancyGrid, globalMap) 75 | #wait if map is not received yet 76 | while (len(mapData.data)<1): 77 | pass 78 | #wait if any of robots' global costmap map is not received yet 79 | for i in range(0,n_robots): 80 | while (len(globalmaps[i].data)<1): 81 | pass 82 | 83 | global_frame="/"+mapData.header.frame_id 84 | 85 | 86 | tfLisn=tf.TransformListener() 87 | if len(namespace) > 0: 88 | for i in range(0,n_robots): 89 | tfLisn.waitForTransform(global_frame[1:], namespace+str(i+namespace_init_count)+'/base_link', rospy.Time(0),rospy.Duration(10.0)) 90 | elif len(namespace)==0: 91 | tfLisn.waitForTransform(global_frame[1:], '/base_link', rospy.Time(0),rospy.Duration(10.0)) 92 | 93 | rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]]) 94 | #pub = rospy.Publisher('frontiers', Marker, queue_size=10) 95 | pub2 = rospy.Publisher('frontiers', Marker, queue_size=10) 96 | filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10) 97 | 98 | rospy.loginfo("the map and global costmaps are received") 99 | 100 | 101 | # wait if no frontier is received yet 102 | while len(frontiers)<1: 103 | pass 104 | 105 | 106 | points=Marker() 107 | points_clust=Marker() 108 | 109 | #Set frame ID and timestamp. See the TF tutorials for information on these. 110 | points.header.frame_id= mapData.header.frame_id 111 | points.header.stamp= rospy.Time.now() 112 | 113 | points.ns= "markers2" 114 | points.id = 0 115 | 116 | points.type = Marker.POINTS 117 | 118 | #Set marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 119 | points.action = Marker.ADD; 120 | 121 | points.pose.orientation.w = 1.0 122 | 123 | points.scale.x=0.2 124 | points.scale.y=0.2 125 | 126 | points.color.r = 255.0/255.0 127 | points.color.g = 255.0/255.0 128 | points.color.b = 0.0/255.0 129 | 130 | points.color.a=1; 131 | points.lifetime = rospy.Duration(); 132 | 133 | p=Point() 134 | 135 | p.z = 0; 136 | 137 | pp=[] 138 | pl=[] 139 | 140 | points_clust.header.frame_id= mapData.header.frame_id 141 | points_clust.header.stamp= rospy.Time.now() 142 | 143 | points_clust.ns= "markers3" 144 | points_clust.id = 4 145 | 146 | points_clust.type = Marker.POINTS 147 | 148 | #Set marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 149 | points_clust.action = Marker.ADD; 150 | 151 | points_clust.pose.orientation.w = 1.0; 152 | 153 | points_clust.scale.x=0.2; 154 | points_clust.scale.y=0.2; 155 | points_clust.color.r = 0.0/255.0 156 | points_clust.color.g = 255.0/255.0 157 | points_clust.color.b = 0.0/255.0 158 | 159 | points_clust.color.a=1; 160 | points_clust.lifetime = rospy.Duration(); 161 | 162 | 163 | temppoint=PointStamped() 164 | temppoint.header.frame_id= mapData.header.frame_id 165 | temppoint.header.stamp=rospy.Time(0) 166 | temppoint.point.z=0.0 167 | 168 | arraypoints=PointArray() 169 | tempPoint=Point() 170 | tempPoint.z=0.0 171 | #--------------------------------------------------------------------- 172 | #----------------- Main Loop ------------------------------- 173 | #--------------------------------------------------------------------- 174 | while not rospy.is_shutdown(): 175 | #------------------------------------------------------------------------- 176 | #Clustering frontier points 177 | ''' 178 | #centroids=[] 179 | #front=copy(frontiers) 180 | #if len(front)>1: 181 | # ms = MeanShift(bandwidth=0.3) 182 | # ms.fit(front) 183 | # centroids= ms.cluster_centers_ #centroids array is the centers of each cluster 184 | 185 | #if there is only one frontier no need for clustering, i.e. centroids=frontiers 186 | #if len(front)==1: 187 | # centroids=front 188 | #frontiers=copy(centroids) 189 | ''' 190 | #--------------------------------------------------------------------- 191 | #clearing old frontiers 192 | 193 | for frontier in list(frontiers): 194 | cond = False 195 | temppoint.point.x=frontier[0] 196 | temppoint.point.y=frontier[1] 197 | 198 | for i in range(0,n_robots): 199 | 200 | transformedPoint=tfLisn.transformPoint(globalmaps[i].header.frame_id,temppoint) 201 | x=(transformedPoint.point.x,transformedPoint.point.y) 202 | cond=(gridValue(globalmaps[i],x)>threshold) or cond 203 | if (informationGain(mapData, [frontier[0], frontier[1]],info_radius*0.5) < 0.2): 204 | if (frontier[0], frontier[1]) in frontiers: 205 | frontiers.remove((frontier[0], frontier[1])) 206 | 207 | ''' 208 | if (cond or (informationGain(mapData,[frontier[0],frontier[1]],info_radius*0.5))<0.2): 209 | if (frontier[0], frontier[1]) in frontiers: 210 | frontiers.remove((frontier[0], frontier[1])) 211 | ''' 212 | 213 | #--------------------------------------------------------------------- 214 | #publishing 215 | arraypoints.points=[] 216 | 217 | for i in list(frontiers): 218 | tempPoint.x=i[0] 219 | tempPoint.y=i[1] 220 | arraypoints.points.append(copy(tempPoint)) 221 | filterpub.publish(arraypoints) 222 | 223 | ''' 224 | pp=[] 225 | for q in range(0,len(frontiers)): 226 | p.x=frontiers[q][0] 227 | p.y=frontiers[q][1] 228 | pp.append(copy(p)) 229 | points.points=pp 230 | ''' 231 | ''' 232 | for q in list(frontiers): 233 | p.x= q[0] 234 | p.y= q[1] 235 | pp.append(copy(p)) 236 | ''' 237 | points_clust.points= copy(arraypoints.points) 238 | 239 | # pub.publish(points) 240 | pub2.publish(points_clust) 241 | rate.sleep() 242 | #------------------------------------------------------------------------- 243 | 244 | if __name__ == '__main__': 245 | try: 246 | node() 247 | except rospy.ROSInterruptException: 248 | pass 249 | -------------------------------------------------------------------------------- /scripts/newassigner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | #--------Include modules--------------- 4 | from copy import copy 5 | import rospy 6 | from visualization_msgs.msg import Marker 7 | from geometry_msgs.msg import Point 8 | from nav_msgs.msg import OccupancyGrid 9 | import tf 10 | from rrt_exploration.msg import PointArray 11 | from time import time 12 | from numpy import array, delete 13 | from numpy import linalg as LA 14 | from numpy import all as All 15 | from numpy import inf 16 | from functions import robot,informationGain,discount 17 | from numpy.linalg import norm 18 | import time 19 | 20 | # Subscribers' callbacks------------------------------ 21 | mapData=OccupancyGrid() 22 | frontiers=[] 23 | global1=OccupancyGrid() 24 | global2=OccupancyGrid() 25 | global3=OccupancyGrid() 26 | globalmaps=[] 27 | def callBack(data): 28 | global frontiers 29 | frontiers=[] 30 | for point in data.points: 31 | frontiers.append(array([point.x,point.y])) 32 | 33 | def mapCallBack(data): 34 | global mapData 35 | mapData=data 36 | # Node---------------------------------------------- 37 | 38 | def node(): 39 | 40 | stop = 0 41 | 42 | global frontiers,mapData,global1,global2,global3,globalmaps 43 | rospy.init_node('assigner', anonymous=False) 44 | 45 | # fetching all parameters 46 | map_topic= rospy.get_param('~map_topic','/map_merge/map') 47 | info_radius= rospy.get_param('~info_radius',1.0) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate 48 | info_multiplier=rospy.get_param('~info_multiplier',3.0) 49 | hysteresis_radius=rospy.get_param('~hysteresis_radius',1.0) #at least as much as the laser scanner range 50 | hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region 51 | frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points') 52 | n_robots = rospy.get_param('~n_robots',1) 53 | namespace = rospy.get_param('~namespace','') 54 | namespace_init_count = rospy.get_param('namespace_init_count',1) 55 | delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5) 56 | rateHz = rospy.get_param('~rate',2) 57 | 58 | rate = rospy.Rate(rateHz) 59 | #------------------------------------------- 60 | rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) 61 | rospy.Subscriber(frontiers_topic, PointArray, callBack) 62 | #--------------------------------------------------------------------------------------------------------------- 63 | 64 | # wait if no frontier is received yet 65 | while len(frontiers)<1: 66 | pass 67 | centroids=copy(frontiers) 68 | #wait if map is not received yet 69 | while (len(mapData.data)<1): 70 | pass 71 | 72 | robots=[] 73 | if len(namespace)>0: 74 | for i in range(0,n_robots): 75 | robots.append(robot(namespace+str(i+namespace_init_count))) 76 | elif len(namespace)==0: 77 | robots.append(robot(namespace)) 78 | for i in range(0,n_robots): 79 | robots[i].sendGoal(robots[i].getPosition()) 80 | 81 | 82 | start = time.time() 83 | #------------------------------------------------------------------------- 84 | #-------------------- Initial Rotation -------------------------- 85 | 86 | if n_robots == 1: 87 | for i in range(0,n_robots): 88 | robots[i].sendGoal(robots[i].getPosition(), rotation=True) 89 | else: 90 | for i in range(0,n_robots): 91 | robots[i].sendGoal(robots[i].getPosition()) 92 | 93 | rospy.sleep(3); 94 | rospy.loginfo("initial finished.") 95 | 96 | 97 | #------------------------------------------------------------------------- 98 | #--------------------- Main Loop ------------------------------- 99 | #------------------------------------------------------------------------- 100 | while not rospy.is_shutdown(): 101 | centroids=copy(frontiers) 102 | #------------------------------------------------------------------------- 103 | #Get information gain for each assigned point 104 | for ip in range(n_robots): 105 | if (robots[ip].getState() == 1): 106 | infoGain = informationGain(mapData,[robots[ip].assigned_point[0],robots[ip].assigned_point[1]], 0.3) 107 | if infoGain < 0.1: 108 | rospy.logwarn("area has been finished, cancel robot_" +str(ip) + "'s goal.") 109 | robots[ip].cancelGoal() 110 | rospy.sleep(delay_after_assignement) 111 | 112 | #get number of available/busy robots 113 | na=[] #available robots 114 | nb=[] #busy robots 115 | for i in range(0,n_robots): 116 | if (robots[i].getState()==1): 117 | nb.append(i) 118 | else: 119 | na.append(i) 120 | if len(na) > 0: 121 | rospy.loginfo("available robots: "+str(na)) 122 | #------------------------------------------------------------------------- 123 | 124 | ''' 125 | 126 | #get dicount and update informationGain 127 | for i in nb+na: 128 | infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius) 129 | #------------------------------------------------------------------------- 130 | ''' 131 | 132 | ip = 0 133 | while ip < len(centroids): 134 | for ir in nb: 135 | if (norm(robots[ir].assigned_point-centroids[ip])<=hysteresis_radius): 136 | centroids=delete(centroids, (ip), axis=0) 137 | ip -= 1 138 | break 139 | ip += 1 140 | 141 | 142 | revenue_record=[] 143 | centroid_record=[] 144 | id_record=[] 145 | 146 | 147 | for ir in na: 148 | for ip in range(0,len(centroids)): 149 | cost=norm(robots[ir].getPosition()-centroids[ip]) 150 | # information_gain=infoGain[ip] 151 | # if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): 152 | 153 | # information_gain*=hysteresis_gain 154 | #revenue=information_gain*info_multiplier-cost 155 | revenue=-cost 156 | 157 | revenue_record.append(revenue) 158 | centroid_record.append(centroids[ip]) 159 | id_record.append(ir) 160 | 161 | rospy.loginfo("revenue record: "+str(revenue_record)) 162 | rospy.loginfo("centroid record: "+str(centroid_record)) 163 | rospy.loginfo("robot IDs record: "+str(id_record)) 164 | ''' 165 | if len(na)<1: 166 | revenue_record=[] 167 | centroid_record=[] 168 | id_record=[] 169 | for ir in nb: 170 | for ip in range(0,len(centroids)): 171 | cost=norm(robots[ir].getPosition()-centroids[ip]) 172 | threshold=1 173 | information_gain=infoGain[ip] 174 | if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): 175 | information_gain*=hysteresis_gain 176 | 177 | if ((norm(centroids[ip]-robots[ir].assigned_point))0): 189 | winner_id=revenue_record.index(max(revenue_record)) 190 | robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) 191 | rospy.loginfo(namespace+str(namespace_init_count+id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) 192 | rospy.sleep(delay_after_assignement) 193 | 194 | if (len(frontiers) > 0): 195 | sub = list(range(n_robots)) 196 | for ir in na: 197 | cost = 1e10 198 | for ri in nb: 199 | if (norm(robots[ir].getPosition()-robots[ri].assigned_point) < norm(robots[ri].getPosition()-robots[ri].assigned_point)) and (cost > norm(robots[ri].getPosition()-robots[ri].assigned_point)): 200 | sub[ri] = ir 201 | cost = norm(robots[ri].getPosition()-robots[ri].assigned_point) 202 | for i in range(len(sub)): 203 | if sub[i] != i: 204 | rospy.loginfo(namespace+str(namespace_init_count+sub[i])+" sub " + str(i) + " assigned to "+str(robots[i].assigned_point)) 205 | 206 | robots[sub[i]].sendGoal(robots[i].assigned_point) 207 | robots[i].cancelGoal() 208 | rospy.sleep(delay_after_assignement) 209 | 210 | 211 | if (len(frontiers) != 0): 212 | stop = 0 213 | else: 214 | stop += 1 215 | 216 | if (stop >= rateHz*2): 217 | end = time.time() 218 | rospy.loginfo("total time: " + str(end-start)) 219 | stop = 0 220 | #------------------------------------------------------------------------- 221 | rate.sleep() 222 | #------------------------------------------------------------------------- 223 | 224 | 225 | if __name__ == '__main__': 226 | try: 227 | node() 228 | except rospy.ROSInterruptException: 229 | end = time.time() 230 | rospy.loginfo("total time: " + str(end-start)) 231 | rospy.loginfo("Exploration has been finished.") 232 | 233 | 234 | 235 | 236 | -------------------------------------------------------------------------------- /scripts/functions.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import tf 3 | from numpy import array 4 | import actionlib 5 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 6 | from nav_msgs.srv import GetPlan 7 | from geometry_msgs.msg import PoseStamped, Quaternion 8 | from numpy import floor 9 | from numpy.linalg import norm 10 | from numpy import inf 11 | from tf.transformations import quaternion_from_euler 12 | #________________________________________________________________________________ 13 | class robot: 14 | goal = MoveBaseGoal() 15 | start = PoseStamped() 16 | end = PoseStamped() 17 | 18 | def __init__(self,name): 19 | self.assigned_point=[] 20 | self.name=name 21 | self.global_frame=rospy.get_param('~global_frame','map') 22 | self.listener=tf.TransformListener() 23 | self.listener.waitForTransform(self.global_frame, name+'/base_footprint', rospy.Time(0),rospy.Duration(10.0)) 24 | cond=0; 25 | while cond==0: 26 | try: 27 | (trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_footprint', rospy.Time(0)) 28 | cond=1 29 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 30 | cond==0 31 | self.position=array([trans[0],trans[1]]) 32 | self.assigned_point=self.position 33 | self.client=actionlib.SimpleActionClient(self.name+'/move_base', MoveBaseAction) 34 | self.client.wait_for_server() 35 | robot.goal.target_pose.header.frame_id=self.global_frame 36 | robot.goal.target_pose.header.stamp=rospy.Time.now() 37 | 38 | rospy.wait_for_service(self.name+'/move_base/NavfnROS/make_plan') 39 | self.make_plan = rospy.ServiceProxy(self.name+'/move_base/NavfnROS/make_plan', GetPlan) 40 | robot.start.header.frame_id=self.global_frame 41 | robot.end.header.frame_id=self.global_frame 42 | 43 | def getPosition(self): 44 | cond=0; 45 | while cond==0: 46 | try: 47 | (trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_link', rospy.Time(0)) 48 | cond=1 49 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 50 | cond==0 51 | self.position=array([trans[0],trans[1]]) 52 | return self.position 53 | 54 | def sendGoal(self,point, rotation=False): 55 | robot.goal.target_pose.pose.position.x=point[0] 56 | robot.goal.target_pose.pose.position.y=point[1] 57 | if rotation: 58 | # robot.goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 3.1415926)) 59 | robot.goal.target_pose.pose.orientation.z = 1.0 60 | robot.goal.target_pose.pose.orientation.w = 0.0 61 | else: 62 | robot.goal.target_pose.pose.orientation.w = 1.0 63 | robot.goal.target_pose.pose.orientation.z = 0.0 64 | self.client.send_goal(robot.goal) 65 | self.assigned_point=array(point) 66 | 67 | def cancelGoal(self): 68 | self.client.cancel_goal() 69 | self.assigned_point=self.getPosition() 70 | 71 | def getState(self): 72 | return self.client.get_state() 73 | 74 | def makePlan(self,start,end): 75 | robot.start.pose.position.x=start[0] 76 | robot.start.pose.position.y=start[1] 77 | robot.end.pose.position.x=end[0] 78 | robot.end.pose.position.y=end[1] 79 | start=self.listener.transformPose(self.name+'/map', robot.start) 80 | end=self.listener.transformPose(self.name+'/map', robot.end) 81 | plan=self.make_plan(start = start, goal = end, tolerance = 0.0) 82 | return plan.plan.poses 83 | #________________________________________________________________________________ 84 | 85 | def index_of_point(mapData,Xp): 86 | resolution=mapData.info.resolution 87 | Xstartx=mapData.info.origin.position.x 88 | Xstarty=mapData.info.origin.position.y 89 | width=mapData.info.width 90 | Data=mapData.data 91 | index=int( ( floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) )) 92 | return index 93 | 94 | def point_of_index(mapData,i): 95 | y=mapData.info.origin.position.y+(i/mapData.info.width)*mapData.info.resolution 96 | x=mapData.info.origin.position.x+(i-(i/mapData.info.width)*(mapData.info.width))*mapData.info.resolution 97 | return array([x,y]) 98 | #________________________________________________________________________________ 99 | 100 | def informationGain(mapData,point,r): 101 | infoGain=0; 102 | index=index_of_point(mapData,point) 103 | r_region=int(r/mapData.info.resolution) 104 | init_index=index-r_region*(mapData.info.width+1) 105 | for n in range(0,2*r_region+1): 106 | start=n*mapData.info.width+init_index 107 | end=start+2*r_region 108 | limit=((start/mapData.info.width)+2)*mapData.info.width 109 | for i in range(start,end+1): 110 | if (i>=0 and i=0 and i0): 135 | i=len(path)/2 136 | p1=array([path[i-1].pose.position.x,path[i-1].pose.position.y]) 137 | p2=array([path[i].pose.position.x,path[i].pose.position.y]) 138 | return norm(p1-p2)*(len(path)-1) 139 | else: 140 | return inf 141 | #________________________________________________________________________________ 142 | 143 | def unvalid(mapData,pt): 144 | index=index_of_point(mapData,pt) 145 | r_region=5 146 | init_index=index-r_region*(mapData.info.width+1) 147 | for n in range(0,2*r_region+1): 148 | start=n*mapData.info.width+init_index 149 | end=start+2*r_region 150 | limit=((start/mapData.info.width)+2)*mapData.info.width 151 | for i in range(start,end+1): 152 | if (i>=0 and i width*height-1): 203 | rospy.logwarn("Evaluating nhood for offmap point") 204 | return out 205 | 206 | if (index % width > 0): 207 | out.append(index-1) 208 | 209 | if (index % width < (width-1)): 210 | out.append(index+1) 211 | 212 | if (index >= width): 213 | out.append(index-width) 214 | 215 | if (index < width*(height-1)): 216 | out.append(index+width) 217 | 218 | if (index % width > 0 and index >= width): 219 | out.append(index-1-width) 220 | 221 | if (index % width > 0 and index < width*(height-1)): 222 | out.append(index-1+width) 223 | 224 | if (index % width < (width-1) and index >= width): 225 | out.append(index+1-width) 226 | 227 | if (index % width < (width-1) and index < width*(height-1)): 228 | out.append(index+1+width) 229 | 230 | return out 231 | 232 | def gridValue(mapData,Xp): 233 | resolution=mapData.info.resolution 234 | Xstartx=mapData.info.origin.position.x 235 | Xstarty=mapData.info.origin.position.y 236 | 237 | width=mapData.info.width 238 | Data=mapData.data 239 | # returns grid value at "Xp" location 240 | #map data: 100 occupied -1 unknown 0 free 241 | index=( floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) ) 242 | 243 | if int(index) < len(Data): 244 | return Data[int(index)] 245 | else: 246 | return 100 247 | -------------------------------------------------------------------------------- /scripts/functions_real_two_robots.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import tf 3 | from numpy import array 4 | import actionlib 5 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 6 | from nav_msgs.srv import GetPlan 7 | from geometry_msgs.msg import PoseStamped, Quaternion 8 | from numpy import floor 9 | from numpy.linalg import norm 10 | from numpy import inf 11 | from tf.transformations import quaternion_from_euler 12 | #________________________________________________________________________________ 13 | class robot: 14 | goal = MoveBaseGoal() 15 | start = PoseStamped() 16 | end = PoseStamped() 17 | 18 | def __init__(self,name): 19 | self.assigned_point=[] 20 | self.name=name 21 | self.global_frame=rospy.get_param('~global_frame','/map') 22 | self.listener=tf.TransformListener() 23 | self.listener.waitForTransform(self.global_frame, name+'/base_footprint', rospy.Time(0),rospy.Duration(10.0)) 24 | cond=0; 25 | while cond==0: 26 | try: 27 | (trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_footprint', rospy.Time(0)) 28 | cond=1 29 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 30 | cond==0 31 | self.position=array([trans[0],trans[1]]) 32 | self.assigned_point=self.position 33 | self.client=actionlib.SimpleActionClient(self.name+'/move_base', MoveBaseAction) 34 | self.client.wait_for_server() 35 | robot.goal.target_pose.header.frame_id=self.global_frame 36 | robot.goal.target_pose.header.stamp=rospy.Time.now() 37 | 38 | rospy.wait_for_service(self.name+'/move_base/NavfnROS/make_plan') 39 | self.make_plan = rospy.ServiceProxy(self.name+'/move_base/NavfnROS/make_plan', GetPlan) 40 | robot.start.header.frame_id=self.global_frame 41 | robot.end.header.frame_id=self.global_frame 42 | 43 | def getPosition(self): 44 | cond=0; 45 | while cond==0: 46 | try: 47 | (trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_footprint', rospy.Time(0)) 48 | cond=1 49 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 50 | cond==0 51 | self.position=array([trans[0],trans[1]]) 52 | return self.position 53 | 54 | def sendGoal(self,point, rotation=False): 55 | robot.goal.target_pose.pose.position.x=point[0] 56 | robot.goal.target_pose.pose.position.y=point[1] 57 | if rotation: 58 | # robot.goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 3.1415926)) 59 | robot.goal.target_pose.pose.orientation.z = 1.0 60 | robot.goal.target_pose.pose.orientation.w = 0.0 61 | else: 62 | robot.goal.target_pose.pose.orientation.w = 1.0 63 | robot.goal.target_pose.pose.orientation.z = 0.0 64 | self.client.send_goal(robot.goal) 65 | self.assigned_point=array(point) 66 | 67 | def cancelGoal(self): 68 | self.client.cancel_goal() 69 | self.assigned_point=self.getPosition() 70 | 71 | def getState(self): 72 | return self.client.get_state() 73 | 74 | def makePlan(self,start,end): 75 | robot.start.pose.position.x=start[0] 76 | robot.start.pose.position.y=start[1] 77 | robot.end.pose.position.x=end[0] 78 | robot.end.pose.position.y=end[1] 79 | start=self.listener.transformPose(self.name+'/map', robot.start) 80 | end=self.listener.transformPose(self.name+'/map', robot.end) 81 | plan=self.make_plan(start = start, goal = end, tolerance = 0.0) 82 | return plan.plan.poses 83 | #________________________________________________________________________________ 84 | 85 | def index_of_point(mapData,Xp): 86 | resolution=mapData.info.resolution 87 | Xstartx=mapData.info.origin.position.x 88 | Xstarty=mapData.info.origin.position.y 89 | width=mapData.info.width 90 | Data=mapData.data 91 | index=int( ( floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) )) 92 | return index 93 | 94 | def point_of_index(mapData,i): 95 | y=mapData.info.origin.position.y+(i/mapData.info.width)*mapData.info.resolution 96 | x=mapData.info.origin.position.x+(i-(i/mapData.info.width)*(mapData.info.width))*mapData.info.resolution 97 | return array([x,y]) 98 | #________________________________________________________________________________ 99 | 100 | def informationGain(mapData,point,r): 101 | infoGain=0; 102 | index=index_of_point(mapData,point) 103 | r_region=int(r/mapData.info.resolution) 104 | init_index=index-r_region*(mapData.info.width+1) 105 | for n in range(0,2*r_region+1): 106 | start=n*mapData.info.width+init_index 107 | end=start+2*r_region 108 | limit=((start/mapData.info.width)+2)*mapData.info.width 109 | for i in range(start,end+1): 110 | if (i>=0 and i=0 and i0): 135 | i=len(path)/2 136 | p1=array([path[i-1].pose.position.x,path[i-1].pose.position.y]) 137 | p2=array([path[i].pose.position.x,path[i].pose.position.y]) 138 | return norm(p1-p2)*(len(path)-1) 139 | else: 140 | return inf 141 | #________________________________________________________________________________ 142 | 143 | def unvalid(mapData,pt): 144 | index=index_of_point(mapData,pt) 145 | r_region=5 146 | init_index=index-r_region*(mapData.info.width+1) 147 | for n in range(0,2*r_region+1): 148 | start=n*mapData.info.width+init_index 149 | end=start+2*r_region 150 | limit=((start/mapData.info.width)+2)*mapData.info.width 151 | for i in range(start,end+1): 152 | if (i>=0 and i width*height-1): 203 | rospy.logwarn("Evaluating nhood for offmap point") 204 | return out 205 | 206 | if (index % width > 0): 207 | out.append(index-1) 208 | 209 | if (index % width < (width-1)): 210 | out.append(index+1) 211 | 212 | if (index >= width): 213 | out.append(index-width) 214 | 215 | if (index < width*(height-1)): 216 | out.append(index+width) 217 | 218 | if (index % width > 0 and index >= width): 219 | out.append(index-1-width) 220 | 221 | if (index % width > 0 and index < width*(height-1)): 222 | out.append(index-1+width) 223 | 224 | if (index % width < (width-1) and index >= width): 225 | out.append(index+1-width) 226 | 227 | if (index % width < (width-1) and index < width*(height-1)): 228 | out.append(index+1+width) 229 | 230 | return out 231 | 232 | def gridValue(mapData,Xp): 233 | resolution=mapData.info.resolution 234 | Xstartx=mapData.info.origin.position.x 235 | Xstarty=mapData.info.origin.position.y 236 | 237 | width=mapData.info.width 238 | Data=mapData.data 239 | # returns grid value at "Xp" location 240 | #map data: 100 occupied -1 unknown 0 free 241 | index=( floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) ) 242 | 243 | if int(index) < len(Data): 244 | return Data[int(index)] 245 | else: 246 | return 100 247 | --------------------------------------------------------------------------------