diff --git a/CMakeLists.txt b/CMakeLists.txt index 9b69333..78fb39a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,7 +39,7 @@ CATKIN_DEPENDS message_runtime include_directories(include ${catkin_INCLUDE_DIRS}) -add_executable(rrt_detector src/rrt_detector.cpp src/functions.cpp src/mtrand.cpp) +add_executable(global_rrt_detector src/global_rrt_detector.cpp src/functions.cpp src/mtrand.cpp) target_link_libraries(rrt_detector ${catkin_LIBRARIES}) add_executable(local_rrt_detector src/local_rrt_detector.cpp src/functions.cpp src/mtrand.cpp) diff --git a/CMakeLists.txt~ b/CMakeLists.txt~ deleted file mode 100644 index d5dbb67..0000000 --- a/CMakeLists.txt~ +++ /dev/null @@ -1,46 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rrt_exploration) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs - tf - visualization_msgs - message_generation -) - - -add_message_files( - FILES - PointArray.msg -) - -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - - -catkin_package( - -CATKIN_DEPENDS message_runtime geometry_msgs std_msgs -) - - - - - -include_directories(include ${catkin_INCLUDE_DIRS}) - -add_executable(rrt_detector src/rrt_detector.cpp src/functions.cpp src/mtrand.cpp) -target_link_libraries(rrt_detector ${catkin_LIBRARIES}) - -add_executable(local_rrt_detector src/local_rrt_detector.cpp src/functions.cpp src/mtrand.cpp) -target_link_libraries(local_rrt_detector ${catkin_LIBRARIES}) diff --git a/include/functions.h~ b/include/functions.h~ deleted file mode 100644 index 21746a8..0000000 --- a/include/functions.h~ +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef functions_H -#define functions_H -#include "ros/ros.h" -#include -#include -#include -#include -#include "nav_msgs/OccupancyGrid.h" -#include "geometry_msgs/Point.h" -#include "visualization_msgs/Marker.h" - -// rdm class, for gentaring random flot numbers -class rdm{ -int i; -public: -rdm(); -float randomize(); -}; - - -//Norm function prototype -float Norm( std::vector , std::vector ); - -//sign function prototype -float sign(float ); - -//Nearest function prototype -std::vector Nearest( std::vector< std::vector > , std::vector ); - -//Steer function prototype -std::vector Steer( std::vector, std::vector, float ); - -//gridValue function prototype -int gridValue(nav_msgs::OccupancyGrid &,std::vector); - -//ObstacleFree function prototype -char ObstacleFree(std::vector , std::vector & , nav_msgs::OccupancyGrid); -#endif diff --git a/launch/assigner.launch b/launch/assigner.launch deleted file mode 100644 index 00388c5..0000000 --- a/launch/assigner.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/launch/assigner.launch~ b/launch/assigner.launch~ deleted file mode 100644 index 00388c5..0000000 --- a/launch/assigner.launch~ +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/launch/assigner_opencv.launch~ b/launch/assigner_opencv.launch~ deleted file mode 100644 index 0fb3e9f..0000000 --- a/launch/assigner_opencv.launch~ +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/launch/rrt_detector.launch~ b/launch/rrt_detector.launch~ deleted file mode 100644 index 26826f4..0000000 --- a/launch/rrt_detector.launch~ +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/single.launch~ b/launch/single.launch~ deleted file mode 100644 index 2cb79a4..0000000 --- a/launch/single.launch~ +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/msg/PointArray.msg~ b/msg/PointArray.msg~ deleted file mode 100644 index 3f29f94..0000000 --- a/msg/PointArray.msg~ +++ /dev/null @@ -1,3 +0,0 @@ -# An array of points - -geometry_msgs/Point[] points diff --git a/package.xml b/package.xml index 2ca8c32..a4ae9df 100644 --- a/package.xml +++ b/package.xml @@ -1,13 +1,13 @@ rrt_exploration - 0.0.0 + 1.0.0 The rrt_exploration package - hassan + Hassan Umari MIT diff --git a/package.xml~ b/package.xml~ deleted file mode 100644 index 2ca8c32..0000000 --- a/package.xml~ +++ /dev/null @@ -1,40 +0,0 @@ - - - rrt_exploration - 0.0.0 - The rrt_exploration package - - - - - hassan - - MIT - - - - catkin - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs - tf - visualization_msgs - message_generation - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs - tf - visualization_msgs - message_runtime - - - - - - - - diff --git a/scripts/assigner.py~ b/scripts/assigner.py~ deleted file mode 100755 index 4f0f98f..0000000 --- a/scripts/assigner.py~ +++ /dev/null @@ -1,168 +0,0 @@ -#!/usr/bin/env python - -#--------Include modules--------------- -from copy import copy -import rospy -from visualization_msgs.msg import Marker -from std_msgs.msg import String -from geometry_msgs.msg import Point -from nav_msgs.msg import OccupancyGrid -import actionlib_msgs.msg -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PointStamped -from geometry_msgs.msg import PoseStamped -from nav_msgs.srv import GetPlan -import actionlib -import tf -from rrt_exploration.msg import PointArray - -from time import time -from os import system -from random import random -from numpy import array,concatenate,vstack,delete,floor,ceil -from numpy import linalg as LA -from numpy import all as All -from numpy import inf -from functions import gridValue,robot,informationGain,discount,pathCost,unvalid -from sklearn.cluster import KMeans -from sklearn.cluster import MeanShift -import numpy as np -from numpy.linalg import norm - -# Subscribers' callbacks------------------------------ -mapData=OccupancyGrid() -frontiers=[] -global1=OccupancyGrid() -global2=OccupancyGrid() -global3=OccupancyGrid() -globalmaps=[] -def callBack(data): - global frontiers - frontiers=[] - for point in data.points: - frontiers.append(array([point.x,point.y])) - -def mapCallBack(data): - global mapData - mapData=data -# Node---------------------------------------------- - -def node(): - global frontiers,mapData,global1,global2,global3,globalmaps - rospy.init_node('assigner', anonymous=False) - - # fetching all parameters - map_topic= rospy.get_param('~map_topic','/robot_1/map') - 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 - info_multiplier=rospy.get_param('~info_multiplier',3.0) - hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0) #at least as much as the laser scanner range - hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region - frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points') - n_robots = rospy.get_param('~n_robots',1) - delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5) - rateHz = rospy.get_param('~rate',100) - - rate = rospy.Rate(rateHz) -#------------------------------------------- - rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) - rospy.Subscriber(frontiers_topic, PointArray, callBack) -#--------------------------------------------------------------------------------------------------------------- - -# wait if no frontier is received yet - while len(frontiers)<1: - pass - centroids=copy(frontiers) -#wait if map is not received yet - while (len(mapData.data)<1): - pass - - robots=[] - for i in range(0,n_robots): - robots.append(robot('/robot_'+str(i+1))) - for i in range(0,n_robots): - robots[i].sendGoal(robots[i].getPosition()) -#------------------------------------------------------------------------- -#--------------------- Main Loop ------------------------------- -#------------------------------------------------------------------------- - while not rospy.is_shutdown(): - centroids=copy(frontiers) -#------------------------------------------------------------------------- -#Get information gain for each frontier point - infoGain=[] - for ip in range(0,len(centroids)): - infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)) -#------------------------------------------------------------------------- -#get number of available/busy robots - na=[] #available robots - nb=[] #busy robots - for i in range(0,n_robots): - if (robots[i].getState()==1): - nb.append(i) - else: - na.append(i) - rospy.loginfo("available robots: "+str(na)) -#------------------------------------------------------------------------- -#get dicount and update informationGain - for i in nb+na: - infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius) -#------------------------------------------------------------------------- - revenue_record=[] - centroid_record=[] - id_record=[] - - for ir in na: - for ip in range(0,len(centroids)): - cost=norm(robots[ir].getPosition()-centroids[ip]) - threshold=1 - information_gain=infoGain[ip] - if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): - - information_gain*=hysteresis_gain - revenue=information_gain*info_multiplier-cost - revenue_record.append(revenue) - centroid_record.append(centroids[ip]) - id_record.append(ir) - - if len(na)<1: - revenue_record=[] - centroid_record=[] - id_record=[] - for ir in nb: - for ip in range(0,len(centroids)): - cost=norm(robots[ir].getPosition()-centroids[ip]) - threshold=1 - information_gain=infoGain[ip] - if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): - information_gain*=hysteresis_gain - - if ((norm(centroids[ip]-robots[ir].assigned_point))0): - winner_id=revenue_record.index(max(revenue_record)) - robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) - rospy.loginfo("robot_"+str(id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) - rospy.sleep(delay_after_assignement) -#------------------------------------------------------------------------- - rate.sleep() -#------------------------------------------------------------------------- - -if __name__ == '__main__': - try: - node() - except rospy.ROSInterruptException: - pass - - - - diff --git a/scripts/assigner_opencv.py~ b/scripts/assigner_opencv.py~ deleted file mode 100755 index 9e562a4..0000000 --- a/scripts/assigner_opencv.py~ +++ /dev/null @@ -1,286 +0,0 @@ -#!/usr/bin/env python - - -#--------Include modules--------------- -from copy import copy -import rospy -from visualization_msgs.msg import Marker -from std_msgs.msg import String -from geometry_msgs.msg import Point -from nav_msgs.msg import OccupancyGrid -import actionlib_msgs.msg -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PointStamped -from geometry_msgs.msg import PoseStamped -from nav_msgs.srv import GetPlan -import actionlib -import tf - - - -from time import time -from os import system -from random import random -from numpy import array,concatenate,vstack,delete,floor,ceil -from numpy import linalg as LA -from numpy import all as All -from numpy import inf -from functions import gridValue,robot,informationGain,discount,pathCost,unvalid -from sklearn.cluster import KMeans -from sklearn.cluster import MeanShift -import numpy as np -from numpy.linalg import norm - - - - -# Subscribers' callbacks------------------------------ -mapData=OccupancyGrid() -frontiers=[] -global1=OccupancyGrid() -global2=OccupancyGrid() -global3=OccupancyGrid() -globalmaps=[] -def callBack(data): - global frontiers,min_distance - x=[array([data.x,data.y])] - if len(frontiers)>0: - frontiers=vstack((frontiers,x)) - else: - frontiers=x - - -def mapCallBack(data): - global mapData - mapData=data - -def globalMap(data): - global global1,globalmaps - global1=data - indx=int(data._connection_header['topic'][7])-1 - globalmaps[indx]=data - - -# Node---------------------------------------------- - -def node(): - global frontiers,mapData,global1,global2,global3,globalmaps - rospy.init_node('assigner', anonymous=False) - - # fetching all parameters - map_topic= rospy.get_param('~map_topic','/robot_1/map') - 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 - info_multiplier=rospy.get_param('~info_multiplier',3.0) - hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0) #at least as much as the laser scanner range - hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region - goals_topic= rospy.get_param('~goals_topic','/exploration_goals') - n_robots = rospy.get_param('~n_robots',1) -# global_frame=rospy.get_param('~global_frame','/robot_1/map') - - rate = rospy.Rate(100) -#------------------------------------------- - rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) - rospy.Subscriber(goals_topic, Point, callBack) - pub = rospy.Publisher('frontiers', Marker, queue_size=10) - pub2 = rospy.Publisher('centroids', Marker, queue_size=10) -#--------------------------------------------------------------------------------------------------------------- - - - for i in range(0,n_robots): - globalmaps.append(OccupancyGrid()) - - for i in range(0,n_robots): - rospy.Subscriber('/robot_'+str(i+1)+'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) - -# wait if no frontier is received yet - while len(frontiers)<1: - pass -#wait if map is not received yet - while (len(mapData.data)<1): - pass -#wait if any of robots' global costmap map is not received yet - for i in range(0,n_robots): - while (len(globalmaps[i].data)<1): - pass - - global_frame="/"+mapData.header.frame_id - - - rospy.loginfo("the map and global costmaps are received") - - points=Marker() - points_clust=Marker() -#Set the frame ID and timestamp. See the TF tutorials for information on these. - points.header.frame_id= "/"+mapData.header.frame_id - points.header.stamp= rospy.Time.now() - - points.ns= "markers2" - points.id = 0 - - points.type = Marker.POINTS - -#Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) - points.action = Marker.ADD; - - points.pose.orientation.w = 1.0 - - points.scale.x=0.2 - points.scale.y=0.2 - - points.color.r = 255.0/255.0 - points.color.g = 255.0/255.0 - points.color.b = 0.0/255.0 - - points.color.a=1; - points.lifetime = rospy.Duration(); - - p=Point() - - p.z = 0; - - pp=[] - pl=[] - - points_clust.header.frame_id= "/robot_1/map" - points_clust.header.stamp= rospy.Time.now() - - points_clust.ns= "markers3" - points_clust.id = 4 - - points_clust.type = Marker.POINTS - -#Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) - points_clust.action = Marker.ADD; - - points_clust.pose.orientation.w = 1.0; - - points_clust.scale.x=0.2; - points_clust.scale.y=0.2; - points_clust.color.r = 0.0/255.0 - points_clust.color.g = 255.0/255.0 - points_clust.color.b = 0.0/255.0 - - points_clust.color.a=1; - points_clust.lifetime = rospy.Duration(); - - robots=[] - for i in range(0,n_robots): - robots.append(robot('/robot_'+str(i+1))) - for i in range(0,n_robots): - robots[i].sendGoal(robots[i].getPosition()) -#------------------------------------------------------------------------- -#--------------------- Main Loop ------------------------------- -#------------------------------------------------------------------------- - while not rospy.is_shutdown(): -#------------------------------------------------------------------------- -#clearing old frontiers - - z=0 - while zthreshold) or cond - - if (cond or (informationGain(mapData,[frontiers[z][0],frontiers[z][1]],info_radius))<1.0): - frontiers=delete(frontiers, (z), axis=0) - z=z-1 - z+=1 -#------------------------------------------------------------------------- -#Clustering frontier points - centroids=[] - if len(frontiers)>1: - ms = MeanShift(bandwidth=0.5) - ms.fit(frontiers) - centroids= ms.cluster_centers_ #centroids array is the centers of each cluster - - #if there is only one frontier no need for clustering, i.e. centroids=frontiers - if len(frontiers)==1: - centroids=frontiers - -#------------------------------------------------------------------------- -#Get information gain for each frontier point - infoGain=[] - for ip in range(0,len(centroids)): - infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)) -#------------------------------------------------------------------------- -#get number of available/busy robots - na=[] #available robots - nb=[] #busy robots - for i in range(0,n_robots): - if (robots[i].getState()==1): - nb.append(i) - else: - na.append(i) - - - rospy.loginfo("available robots: "+str(na)) -#------------------------------------------------------------------------- -#get dicount and update informationGain - for i in nb+na: - infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius) -#------------------------------------------------------------------------- - revenue_record=[] - centroid_record=[] - id_record=[] - - for ir in nb+na: - for ip in range(0,len(centroids)): - cost=norm(robots[ir].getPosition()-centroids[ip]) - threshold=1 - cond=False - for i in range(0,n_robots): - cond=(gridValue(globalmaps[i],centroids[ip])>threshold) or cond - if cond: - cost=inf - information_gain=infoGain[ip] - if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): - information_gain*=hysteresis_gain - - if (robots[ir].getState()==1 and (norm(centroids[ip]-robots[ir].assigned_point))0): - winner_id=revenue_record.index(max(revenue_record)) - robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) - rospy.loginfo("robot_"+str(id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) - rospy.sleep(0.5) -#------------------------------------------------------------------------- -#Plotting - pp=[] - for q in range(0,len(frontiers)): - p.x=frontiers[q][0] - p.y=frontiers[q][1] - pp.append(copy(p)) - points.points=pp - pp=[] - for q in range(0,len(centroids)): - p.x=centroids[q][0] - p.y=centroids[q][1] - pp.append(copy(p)) - points_clust.points=pp - - pub.publish(points) - pub2.publish(points_clust) - rate.sleep() -#------------------------------------------------------------------------- - -if __name__ == '__main__': - try: - node() - except rospy.ROSInterruptException: - pass - - - - diff --git a/scripts/assigner_testing.py b/scripts/assigner_testing.py deleted file mode 100755 index 4f0f98f..0000000 --- a/scripts/assigner_testing.py +++ /dev/null @@ -1,168 +0,0 @@ -#!/usr/bin/env python - -#--------Include modules--------------- -from copy import copy -import rospy -from visualization_msgs.msg import Marker -from std_msgs.msg import String -from geometry_msgs.msg import Point -from nav_msgs.msg import OccupancyGrid -import actionlib_msgs.msg -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PointStamped -from geometry_msgs.msg import PoseStamped -from nav_msgs.srv import GetPlan -import actionlib -import tf -from rrt_exploration.msg import PointArray - -from time import time -from os import system -from random import random -from numpy import array,concatenate,vstack,delete,floor,ceil -from numpy import linalg as LA -from numpy import all as All -from numpy import inf -from functions import gridValue,robot,informationGain,discount,pathCost,unvalid -from sklearn.cluster import KMeans -from sklearn.cluster import MeanShift -import numpy as np -from numpy.linalg import norm - -# Subscribers' callbacks------------------------------ -mapData=OccupancyGrid() -frontiers=[] -global1=OccupancyGrid() -global2=OccupancyGrid() -global3=OccupancyGrid() -globalmaps=[] -def callBack(data): - global frontiers - frontiers=[] - for point in data.points: - frontiers.append(array([point.x,point.y])) - -def mapCallBack(data): - global mapData - mapData=data -# Node---------------------------------------------- - -def node(): - global frontiers,mapData,global1,global2,global3,globalmaps - rospy.init_node('assigner', anonymous=False) - - # fetching all parameters - map_topic= rospy.get_param('~map_topic','/robot_1/map') - 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 - info_multiplier=rospy.get_param('~info_multiplier',3.0) - hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0) #at least as much as the laser scanner range - hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region - frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points') - n_robots = rospy.get_param('~n_robots',1) - delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5) - rateHz = rospy.get_param('~rate',100) - - rate = rospy.Rate(rateHz) -#------------------------------------------- - rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) - rospy.Subscriber(frontiers_topic, PointArray, callBack) -#--------------------------------------------------------------------------------------------------------------- - -# wait if no frontier is received yet - while len(frontiers)<1: - pass - centroids=copy(frontiers) -#wait if map is not received yet - while (len(mapData.data)<1): - pass - - robots=[] - for i in range(0,n_robots): - robots.append(robot('/robot_'+str(i+1))) - for i in range(0,n_robots): - robots[i].sendGoal(robots[i].getPosition()) -#------------------------------------------------------------------------- -#--------------------- Main Loop ------------------------------- -#------------------------------------------------------------------------- - while not rospy.is_shutdown(): - centroids=copy(frontiers) -#------------------------------------------------------------------------- -#Get information gain for each frontier point - infoGain=[] - for ip in range(0,len(centroids)): - infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)) -#------------------------------------------------------------------------- -#get number of available/busy robots - na=[] #available robots - nb=[] #busy robots - for i in range(0,n_robots): - if (robots[i].getState()==1): - nb.append(i) - else: - na.append(i) - rospy.loginfo("available robots: "+str(na)) -#------------------------------------------------------------------------- -#get dicount and update informationGain - for i in nb+na: - infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius) -#------------------------------------------------------------------------- - revenue_record=[] - centroid_record=[] - id_record=[] - - for ir in na: - for ip in range(0,len(centroids)): - cost=norm(robots[ir].getPosition()-centroids[ip]) - threshold=1 - information_gain=infoGain[ip] - if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): - - information_gain*=hysteresis_gain - revenue=information_gain*info_multiplier-cost - revenue_record.append(revenue) - centroid_record.append(centroids[ip]) - id_record.append(ir) - - if len(na)<1: - revenue_record=[] - centroid_record=[] - id_record=[] - for ir in nb: - for ip in range(0,len(centroids)): - cost=norm(robots[ir].getPosition()-centroids[ip]) - threshold=1 - information_gain=infoGain[ip] - if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): - information_gain*=hysteresis_gain - - if ((norm(centroids[ip]-robots[ir].assigned_point))0): - winner_id=revenue_record.index(max(revenue_record)) - robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) - rospy.loginfo("robot_"+str(id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) - rospy.sleep(delay_after_assignement) -#------------------------------------------------------------------------- - rate.sleep() -#------------------------------------------------------------------------- - -if __name__ == '__main__': - try: - node() - except rospy.ROSInterruptException: - pass - - - - diff --git a/scripts/assigner_testing.py~ b/scripts/assigner_testing.py~ deleted file mode 100755 index 4f0f98f..0000000 --- a/scripts/assigner_testing.py~ +++ /dev/null @@ -1,168 +0,0 @@ -#!/usr/bin/env python - -#--------Include modules--------------- -from copy import copy -import rospy -from visualization_msgs.msg import Marker -from std_msgs.msg import String -from geometry_msgs.msg import Point -from nav_msgs.msg import OccupancyGrid -import actionlib_msgs.msg -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from geometry_msgs.msg import PointStamped -from geometry_msgs.msg import PoseStamped -from nav_msgs.srv import GetPlan -import actionlib -import tf -from rrt_exploration.msg import PointArray - -from time import time -from os import system -from random import random -from numpy import array,concatenate,vstack,delete,floor,ceil -from numpy import linalg as LA -from numpy import all as All -from numpy import inf -from functions import gridValue,robot,informationGain,discount,pathCost,unvalid -from sklearn.cluster import KMeans -from sklearn.cluster import MeanShift -import numpy as np -from numpy.linalg import norm - -# Subscribers' callbacks------------------------------ -mapData=OccupancyGrid() -frontiers=[] -global1=OccupancyGrid() -global2=OccupancyGrid() -global3=OccupancyGrid() -globalmaps=[] -def callBack(data): - global frontiers - frontiers=[] - for point in data.points: - frontiers.append(array([point.x,point.y])) - -def mapCallBack(data): - global mapData - mapData=data -# Node---------------------------------------------- - -def node(): - global frontiers,mapData,global1,global2,global3,globalmaps - rospy.init_node('assigner', anonymous=False) - - # fetching all parameters - map_topic= rospy.get_param('~map_topic','/robot_1/map') - 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 - info_multiplier=rospy.get_param('~info_multiplier',3.0) - hysteresis_radius=rospy.get_param('~hysteresis_radius',3.0) #at least as much as the laser scanner range - hysteresis_gain=rospy.get_param('~hysteresis_gain',2.0) #bigger than 1 (biase robot to continue exploring current region - frontiers_topic= rospy.get_param('~frontiers_topic','/filtered_points') - n_robots = rospy.get_param('~n_robots',1) - delay_after_assignement=rospy.get_param('~delay_after_assignement',0.5) - rateHz = rospy.get_param('~rate',100) - - rate = rospy.Rate(rateHz) -#------------------------------------------- - rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) - rospy.Subscriber(frontiers_topic, PointArray, callBack) -#--------------------------------------------------------------------------------------------------------------- - -# wait if no frontier is received yet - while len(frontiers)<1: - pass - centroids=copy(frontiers) -#wait if map is not received yet - while (len(mapData.data)<1): - pass - - robots=[] - for i in range(0,n_robots): - robots.append(robot('/robot_'+str(i+1))) - for i in range(0,n_robots): - robots[i].sendGoal(robots[i].getPosition()) -#------------------------------------------------------------------------- -#--------------------- Main Loop ------------------------------- -#------------------------------------------------------------------------- - while not rospy.is_shutdown(): - centroids=copy(frontiers) -#------------------------------------------------------------------------- -#Get information gain for each frontier point - infoGain=[] - for ip in range(0,len(centroids)): - infoGain.append(informationGain(mapData,[centroids[ip][0],centroids[ip][1]],info_radius)) -#------------------------------------------------------------------------- -#get number of available/busy robots - na=[] #available robots - nb=[] #busy robots - for i in range(0,n_robots): - if (robots[i].getState()==1): - nb.append(i) - else: - na.append(i) - rospy.loginfo("available robots: "+str(na)) -#------------------------------------------------------------------------- -#get dicount and update informationGain - for i in nb+na: - infoGain=discount(mapData,robots[i].assigned_point,centroids,infoGain,info_radius) -#------------------------------------------------------------------------- - revenue_record=[] - centroid_record=[] - id_record=[] - - for ir in na: - for ip in range(0,len(centroids)): - cost=norm(robots[ir].getPosition()-centroids[ip]) - threshold=1 - information_gain=infoGain[ip] - if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): - - information_gain*=hysteresis_gain - revenue=information_gain*info_multiplier-cost - revenue_record.append(revenue) - centroid_record.append(centroids[ip]) - id_record.append(ir) - - if len(na)<1: - revenue_record=[] - centroid_record=[] - id_record=[] - for ir in nb: - for ip in range(0,len(centroids)): - cost=norm(robots[ir].getPosition()-centroids[ip]) - threshold=1 - information_gain=infoGain[ip] - if (norm(robots[ir].getPosition()-centroids[ip])<=hysteresis_radius): - information_gain*=hysteresis_gain - - if ((norm(centroids[ip]-robots[ir].assigned_point))0): - winner_id=revenue_record.index(max(revenue_record)) - robots[id_record[winner_id]].sendGoal(centroid_record[winner_id]) - rospy.loginfo("robot_"+str(id_record[winner_id])+" assigned to "+str(centroid_record[winner_id])) - rospy.sleep(delay_after_assignement) -#------------------------------------------------------------------------- - rate.sleep() -#------------------------------------------------------------------------- - -if __name__ == '__main__': - try: - node() - except rospy.ROSInterruptException: - pass - - - - diff --git a/scripts/filter.py~ b/scripts/filter.py~ deleted file mode 100755 index 94d9786..0000000 --- a/scripts/filter.py~ +++ /dev/null @@ -1,234 +0,0 @@ -#!/usr/bin/env python - -#--------Include modules--------------- -from copy import copy -import rospy -from visualization_msgs.msg import Marker -from geometry_msgs.msg import Point -from nav_msgs.msg import OccupancyGrid -from geometry_msgs.msg import PointStamped -import tf -from numpy import array,vstack,delete,floor,ceil -from functions import gridValue,informationGain -from sklearn.cluster import KMeans -from sklearn.cluster import MeanShift -from numpy.linalg import norm -from rrt_exploration.msg import PointArray - -# Subscribers' callbacks------------------------------ -mapData=OccupancyGrid() -frontiers=[] -global1=OccupancyGrid() -global2=OccupancyGrid() -global3=OccupancyGrid() -globalmaps=[] -def callBack(data,args): - global frontiers,min_distance - transformedPoint=args[0].transformPoint(args[1],data) - x=[array([transformedPoint.point.x,transformedPoint.point.y])] - if len(frontiers)>0: - frontiers=vstack((frontiers,x)) - else: - frontiers=x - - -def mapCallBack(data): - global mapData - mapData=data - -def globalMap(data): - global global1,globalmaps - global1=data - indx=int(data._connection_header['topic'][7])-1 - globalmaps[indx]=data - -# Node---------------------------------------------- -def node(): - global frontiers,mapData,global1,global2,global3,globalmaps - rospy.init_node('filter', anonymous=False) - - # fetching all parameters - map_topic= rospy.get_param('~map_topic','/robot_1/map') - threshold= rospy.get_param('~costmap_clearing_threshold',70) - 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 - goals_topic= rospy.get_param('~goals_topic','/detected_points') - n_robots = rospy.get_param('~n_robots',1) - rateHz = rospy.get_param('~rate',100) - - rate = rospy.Rate(rateHz) -#------------------------------------------- - rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack) - - -#--------------------------------------------------------------------------------------------------------------- - - - for i in range(0,n_robots): - globalmaps.append(OccupancyGrid()) - - for i in range(0,n_robots): - rospy.Subscriber('/robot_'+str(i+1)+'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap) - -#wait if map is not received yet - while (len(mapData.data)<1): - pass -#wait if any of robots' global costmap map is not received yet - for i in range(0,n_robots): - while (len(globalmaps[i].data)<1): - pass - - global_frame="/"+mapData.header.frame_id - - - tfLisn=tf.TransformListener() - for i in range(0,n_robots): - tfLisn.waitForTransform(global_frame[1:], 'robot_'+str(i+1)+'/base_link', rospy.Time(0),rospy.Duration(10.0)) - - rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]]) - pub = rospy.Publisher('frontiers', Marker, queue_size=10) - pub2 = rospy.Publisher('centroids', Marker, queue_size=10) - filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10) - - rospy.loginfo("the map and global costmaps are received") - - - # wait if no frontier is received yet - while len(frontiers)<1: - pass - - - points=Marker() - points_clust=Marker() -#Set the frame ID and timestamp. See the TF tutorials for information on these. - points.header.frame_id= mapData.header.frame_id - points.header.stamp= rospy.Time.now() - - points.ns= "markers2" - points.id = 0 - - points.type = Marker.POINTS - -#Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) - points.action = Marker.ADD; - - points.pose.orientation.w = 1.0 - - points.scale.x=0.2 - points.scale.y=0.2 - - points.color.r = 255.0/255.0 - points.color.g = 255.0/255.0 - points.color.b = 0.0/255.0 - - points.color.a=1; - points.lifetime = rospy.Duration(); - - p=Point() - - p.z = 0; - - pp=[] - pl=[] - - points_clust.header.frame_id= mapData.header.frame_id - points_clust.header.stamp= rospy.Time.now() - - points_clust.ns= "markers3" - points_clust.id = 4 - - points_clust.type = Marker.POINTS - -#Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) - points_clust.action = Marker.ADD; - - points_clust.pose.orientation.w = 1.0; - - points_clust.scale.x=0.2; - points_clust.scale.y=0.2; - points_clust.color.r = 0.0/255.0 - points_clust.color.g = 255.0/255.0 - points_clust.color.b = 0.0/255.0 - - points_clust.color.a=1; - points_clust.lifetime = rospy.Duration(); - - - temppoint=PointStamped() - temppoint.header.frame_id= mapData.header.frame_id - temppoint.header.stamp=rospy.Time(0) - temppoint.point.z=0.0 - - arraypoints=PointArray() - tempPoint=Point() - tempPoint.z=0.0 -#------------------------------------------------------------------------- -#--------------------- Main Loop ------------------------------- -#------------------------------------------------------------------------- - while not rospy.is_shutdown(): -#------------------------------------------------------------------------- -#Clustering frontier points - centroids=[] - front=copy(frontiers) - if len(front)>1: - ms = MeanShift(bandwidth=0.3) - ms.fit(front) - centroids= ms.cluster_centers_ #centroids array is the centers of each cluster - - #if there is only one frontier no need for clustering, i.e. centroids=frontiers - if len(front)==1: - centroids=front - frontiers=copy(centroids) -#------------------------------------------------------------------------- -#clearing old frontiers - - z=0 - while zthreshold) or cond - if (cond or (informationGain(mapData,[centroids[z][0],centroids[z][1]],info_radius*0.5))<0.2): - centroids=delete(centroids, (z), axis=0) - z=z-1 - z+=1 -#------------------------------------------------------------------------- -#publishing - arraypoints.points=[] - for i in centroids: - tempPoint.x=i[0] - tempPoint.y=i[1] - arraypoints.points.append(copy(tempPoint)) - filterpub.publish(arraypoints) - pp=[] - for q in range(0,len(frontiers)): - p.x=frontiers[q][0] - p.y=frontiers[q][1] - pp.append(copy(p)) - points.points=pp - pp=[] - for q in range(0,len(centroids)): - p.x=centroids[q][0] - p.y=centroids[q][1] - pp.append(copy(p)) - points_clust.points=pp - - pub.publish(points) - pub2.publish(points_clust) - rate.sleep() -#------------------------------------------------------------------------- - -if __name__ == '__main__': - try: - node() - except rospy.ROSInterruptException: - pass - - - - diff --git a/scripts/functions.pyc b/scripts/functions.pyc deleted file mode 100644 index 5120d92..0000000 Binary files a/scripts/functions.pyc and /dev/null differ diff --git a/scripts/functions.py~ b/scripts/functions.py~ deleted file mode 100644 index 6ddbb45..0000000 --- a/scripts/functions.py~ +++ /dev/null @@ -1,222 +0,0 @@ -import rospy -import tf -from numpy import array -import actionlib -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -from nav_msgs.srv import GetPlan -from geometry_msgs.msg import PoseStamped -from numpy import floor -from numpy.linalg import norm -from numpy import inf -from copy import copy -#________________________________________________________________________________ -class robot: - goal = MoveBaseGoal() - start = PoseStamped() - end = PoseStamped() - - def __init__(self,name): - self.assigned_point=[] - self.name=name - self.global_frame=rospy.get_param('~global_frame','/robot_1/map') - self.listener=tf.TransformListener() - self.listener.waitForTransform(self.global_frame, name+'/base_link', rospy.Time(0),rospy.Duration(10.0)) - cond=0; - while cond==0: - try: - (trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_link', rospy.Time(0)) - cond=1 - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - cond==0 - self.position=array([trans[0],trans[1]]) - self.assigned_point=self.position - self.client=actionlib.SimpleActionClient(self.name+'/move_base', MoveBaseAction) - self.client.wait_for_server() - robot.goal.target_pose.header.frame_id=self.global_frame - robot.goal.target_pose.header.stamp=rospy.Time.now() - - rospy.wait_for_service(self.name+'/move_base_node/NavfnROS/make_plan') - self.make_plan = rospy.ServiceProxy(self.name+'/move_base_node/NavfnROS/make_plan', GetPlan) - robot.start.header.frame_id=self.global_frame - robot.end.header.frame_id=self.global_frame - - def getPosition(self): - cond=0; - while cond==0: - try: - (trans,rot) = self.listener.lookupTransform(self.global_frame, self.name+'/base_link', rospy.Time(0)) - cond=1 - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - cond==0 - self.position=array([trans[0],trans[1]]) - return self.position - - def sendGoal(self,point): - robot.goal.target_pose.pose.position.x=point[0] - robot.goal.target_pose.pose.position.y=point[1] - robot.goal.target_pose.pose.orientation.w = 1.0 - self.client.send_goal(robot.goal) - self.assigned_point=array(point) - - def cancelGoal(self): - self.client.cancel_goal() - self.assigned_point=self.getPosition() - - def getState(self): - return self.client.get_state() - - def makePlan(self,start,end): - robot.start.pose.position.x=start[0] - robot.start.pose.position.y=start[1] - robot.end.pose.position.x=end[0] - robot.end.pose.position.y=end[1] - start=self.listener.transformPose(self.name+'/map', robot.start) - end=self.listener.transformPose(self.name+'/map', robot.end) - plan=self.make_plan(start = start, goal = end, tolerance = 0.0) - return plan.plan.poses -#________________________________________________________________________________ - -def index_of_point(mapData,Xp): - resolution=mapData.info.resolution - Xstartx=mapData.info.origin.position.x - Xstarty=mapData.info.origin.position.y - width=mapData.info.width - Data=mapData.data - index=int( ( floor((Xp[1]-Xstarty)/resolution)*width)+( floor((Xp[0]-Xstartx)/resolution) )) - return index - -def point_of_index(mapData,i): - y=mapData.info.origin.position.y+(i/mapData.info.width)*mapData.info.resolution - x=mapData.info.origin.position.x+(i-(i/mapData.info.width)*(mapData.info.width))*mapData.info.resolution - return array([x,y]) -#________________________________________________________________________________ - -def informationGain(mapData,point,r): - infoGain=0; - index=index_of_point(mapData,point) - r_region=int(r/mapData.info.resolution) - init_index=index-r_region*(mapData.info.width+1) - for n in range(0,2*r_region+1): - start=n*mapData.info.width+init_index - end=start+2*r_region - limit=((start/mapData.info.width)+2)*mapData.info.width - for i in range(start,end+1): - if (i>=0 and i=0 and i0): - i=len(path)/2 - p1=array([path[i-1].pose.position.x,path[i-1].pose.position.y]) - p2=array([path[i].pose.position.x,path[i].pose.position.y]) - return norm(p1-p2)*(len(path)-1) - else: - return inf -#________________________________________________________________________________ - -def unvalid(mapData,pt): - index=index_of_point(mapData,pt) - r_region=5 - init_index=index-r_region*(mapData.info.width+1) - for n in range(0,2*r_region+1): - start=n*mapData.info.width+init_index - end=start+2*r_region - limit=((start/mapData.info.width)+2)*mapData.info.width - for i in range(start,end+1): - if (i>=0 and i -#include -#include -#include -#include "stdint.h" -#include "functions.h" -#include "mtrand.h" - - -#include "nav_msgs/OccupancyGrid.h" -#include "geometry_msgs/PointStamped.h" -#include "std_msgs/Header.h" -#include "nav_msgs/MapMetaData.h" -#include "geometry_msgs/Point.h" -#include "visualization_msgs/Marker.h" -#include - - - -// global variables -nav_msgs::OccupancyGrid mapData; -geometry_msgs::PointStamped clickedpoint; -geometry_msgs::PointStamped exploration_goal; -visualization_msgs::Marker points,line; -float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y; - -rdm r; // for genrating random numbers - - - -//Subscribers callback functions--------------------------------------- -void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) -{ -mapData=*msg; -} - - - -void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg) -{ - -geometry_msgs::Point p; -p.x=msg->point.x; -p.y=msg->point.y; -p.z=msg->point.z; - -points.points.push_back(p); - -} - - - - -int main(int argc, char **argv) -{ - - unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7; - MTRand_int32 irand(init, length); // 32-bit int generator -// this is an example of initializing by an array -// you may use MTRand(seed) with any 32bit integer -// as a seed for a simpler initialization - MTRand drand; // double in [0, 1) generator, already init - -// generate the same numbers as in the original C test program - ros::init(argc, argv, "local_rrt_frontier_detector"); - ros::NodeHandle nh; - - // fetching all parameters - float eta,init_map_x,init_map_y,range; - std::string map_topic,base_frame_topic; - - std::string ns; - ns=ros::this_node::getName(); - - ros::param::param(ns+"/eta", eta, 0.5); - ros::param::param(ns+"/map_topic", map_topic, "/robot_1/map"); - ros::param::param(ns+"/robot_frame", base_frame_topic, "/robot_1/base_link"); -//--------------------------------------------------------------- -ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack); -ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack); - -ros::Publisher targetspub = nh.advertise("/detected_points", 10); -ros::Publisher pub = nh.advertise(ns+"_shapes", 10); - -ros::Rate rate(100); - - -// wait until map is received, when a map is received, mapData.header.seq will not be < 1 -while (mapData.header.seq<1 or mapData.data.size()<1) { ros::spinOnce(); ros::Duration(0.1).sleep();} - - - -//visualizations points and lines.. -points.header.frame_id=mapData.header.frame_id; -line.header.frame_id=mapData.header.frame_id; -points.header.stamp=ros::Time(0); -line.header.stamp=ros::Time(0); - -points.ns=line.ns = "markers"; -points.id = 0; -line.id =1; - - -points.type = points.POINTS; -line.type=line.LINE_LIST; - -//Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) -points.action =points.ADD; -line.action = line.ADD; -points.pose.orientation.w =1.0; -line.pose.orientation.w = 1.0; -line.scale.x = 0.03; -line.scale.y= 0.03; -points.scale.x=0.3; -points.scale.y=0.3; - -line.color.r =255.0/255.0; -line.color.g= 0.0/255.0; -line.color.b =0.0/255.0; -points.color.r = 255.0/255.0; -points.color.g = 0.0/255.0; -points.color.b = 0.0/255.0; -points.color.a=0.3; -line.color.a = 1.0; -points.lifetime = ros::Duration(); -line.lifetime = ros::Duration(); - -geometry_msgs::Point p; - - -while(points.points.size()<5) -{ -ros::spinOnce(); - -pub.publish(points) ; -} - - - - -std::vector temp1; -temp1.push_back(points.points[0].x); -temp1.push_back(points.points[0].y); - -std::vector temp2; -temp2.push_back(points.points[2].x); -temp2.push_back(points.points[0].y); - - -init_map_x=Norm(temp1,temp2); -temp1.clear(); temp2.clear(); - -temp1.push_back(points.points[0].x); -temp1.push_back(points.points[0].y); - -temp2.push_back(points.points[0].x); -temp2.push_back(points.points[2].y); - -init_map_y=Norm(temp1,temp2); -temp1.clear(); temp2.clear(); - -Xstartx=(points.points[0].x+points.points[2].x)*.5; -Xstarty=(points.points[0].y+points.points[2].y)*.5; - - - - - -geometry_msgs::Point trans; -trans=points.points[4]; -std::vector< std::vector > V; -std::vector xnew; -xnew.push_back( trans.x);xnew.push_back( trans.y); -V.push_back(xnew); - -points.points.clear(); -pub.publish(points) ; - - - - - - - -std::vector frontiers; -int i=0; -float xr,yr; -std::vector x_rand,x_nearest,x_new; - -tf::TransformListener listener; -// Main loop -while (ros::ok()){ - - -// Sample free -x_rand.clear(); -xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx; -yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty; - - -x_rand.push_back( xr ); x_rand.push_back( yr ); - - -// Nearest -x_nearest=Nearest(V,x_rand); - -// Steer - -x_new=Steer(x_nearest,x_rand,eta); - - -// ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle -char checking=ObstacleFree(x_nearest,x_new,mapData); - - if (checking==-1){ - - exploration_goal.header.stamp=ros::Time(0); - exploration_goal.header.frame_id=mapData.header.frame_id; - exploration_goal.point.x=x_new[0]; - exploration_goal.point.y=x_new[1]; - exploration_goal.point.z=0.0; - p.x=x_new[0]; - p.y=x_new[1]; - p.z=0.0; - - points.points.push_back(p); - pub.publish(points) ; - targetspub.publish(exploration_goal); - points.points.clear(); - V.clear(); - - - tf::StampedTransform transform; - int temp=0; - while (temp==0){ - try{ - temp=1; - listener.lookupTransform(map_topic, base_frame_topic , ros::Time(0), transform); - } - catch (tf::TransformException ex){ - temp=0; - ros::Duration(0.1).sleep(); - }} - - x_new[0]=transform.getOrigin().x(); - x_new[1]=transform.getOrigin().y(); - V.push_back(x_new); - line.points.clear(); - } - - - else if (checking==1){ - V.push_back(x_new); - - p.x=x_new[0]; - p.y=x_new[1]; - p.z=0.0; - line.points.push_back(p); - p.x=x_nearest[0]; - p.y=x_nearest[1]; - p.z=0.0; - line.points.push_back(p); - - } - - - -pub.publish(line); - - - - -ros::spinOnce(); -rate.sleep(); - }return 0;} diff --git a/src/rrt_detector.cpp~ b/src/rrt_detector.cpp~ deleted file mode 100644 index 285008c..0000000 --- a/src/rrt_detector.cpp~ +++ /dev/null @@ -1,256 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include -#include -#include -#include -#include "stdint.h" -#include "functions.h" -#include "mtrand.h" - - -#include "nav_msgs/OccupancyGrid.h" -#include "geometry_msgs/PointStamped.h" -#include "std_msgs/Header.h" -#include "nav_msgs/MapMetaData.h" -#include "geometry_msgs/Point.h" -#include "visualization_msgs/Marker.h" -#include - - - -// global variables -nav_msgs::OccupancyGrid mapData; -geometry_msgs::PointStamped clickedpoint; -geometry_msgs::PointStamped exploration_goal; -visualization_msgs::Marker points,line; -float xdim,ydim,resolution,Xstartx,Xstarty,init_map_x,init_map_y; - -rdm r; // for genrating random numbers - - - -//Subscribers callback functions--------------------------------------- -void mapCallBack(const nav_msgs::OccupancyGrid::ConstPtr& msg) -{ -mapData=*msg; -} - - - -void rvizCallBack(const geometry_msgs::PointStamped::ConstPtr& msg) -{ - -geometry_msgs::Point p; -p.x=msg->point.x; -p.y=msg->point.y; -p.z=msg->point.z; - -points.points.push_back(p); - -} - - - - -int main(int argc, char **argv) -{ - - unsigned long init[4] = {0x123, 0x234, 0x345, 0x456}, length = 7; - MTRand_int32 irand(init, length); // 32-bit int generator -// this is an example of initializing by an array -// you may use MTRand(seed) with any 32bit integer -// as a seed for a simpler initialization - MTRand drand; // double in [0, 1) generator, already init - -// generate the same numbers as in the original C test program - ros::init(argc, argv, "rrt_frontier_detector"); - ros::NodeHandle nh; - - // fetching all parameters - float eta,init_map_x,init_map_y,range; - std::string map_topic,base_frame_topic; - - std::string ns; - ns=ros::this_node::getName(); - - ros::param::param(ns+"/eta", eta, 0.5); - ros::param::param(ns+"/map_topic", map_topic, "/robot_1/map"); -//--------------------------------------------------------------- -ros::Subscriber sub= nh.subscribe(map_topic, 100 ,mapCallBack); -ros::Subscriber rviz_sub= nh.subscribe("/clicked_point", 100 ,rvizCallBack); - -ros::Publisher targetspub = nh.advertise("/detected_points", 10); -ros::Publisher pub = nh.advertise("shapes", 10); - -ros::Rate rate(100); - - -// wait until map is received, when a map is received, mapData.header.seq will not be < 1 -while (mapData.header.seq<1 or mapData.data.size()<1) { ros::spinOnce(); ros::Duration(0.1).sleep();} - - - -//visualizations points and lines.. -points.header.frame_id=mapData.header.frame_id; -line.header.frame_id=mapData.header.frame_id; -points.header.stamp=ros::Time(0); -line.header.stamp=ros::Time(0); - -points.ns=line.ns = "markers"; -points.id = 0; -line.id =1; - - -points.type = points.POINTS; -line.type=line.LINE_LIST; - -//Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) -points.action =points.ADD; -line.action = line.ADD; -points.pose.orientation.w =1.0; -line.pose.orientation.w = 1.0; -line.scale.x = 0.03; -line.scale.y= 0.03; -points.scale.x=0.3; -points.scale.y=0.3; - -line.color.r =9.0/255.0; -line.color.g= 91.0/255.0; -line.color.b =236.0/255.0; -points.color.r = 255.0/255.0; -points.color.g = 0.0/255.0; -points.color.b = 0.0/255.0; -points.color.a=1.0; -line.color.a = 1.0; -points.lifetime = ros::Duration(); -line.lifetime = ros::Duration(); - -geometry_msgs::Point p; - - -while(points.points.size()<5) -{ -ros::spinOnce(); - -pub.publish(points) ; -} - - - - -std::vector temp1; -temp1.push_back(points.points[0].x); -temp1.push_back(points.points[0].y); - -std::vector temp2; -temp2.push_back(points.points[2].x); -temp2.push_back(points.points[0].y); - - -init_map_x=Norm(temp1,temp2); -temp1.clear(); temp2.clear(); - -temp1.push_back(points.points[0].x); -temp1.push_back(points.points[0].y); - -temp2.push_back(points.points[0].x); -temp2.push_back(points.points[2].y); - -init_map_y=Norm(temp1,temp2); -temp1.clear(); temp2.clear(); - -Xstartx=(points.points[0].x+points.points[2].x)*.5; -Xstarty=(points.points[0].y+points.points[2].y)*.5; - - - - - -geometry_msgs::Point trans; -trans=points.points[4]; -std::vector< std::vector > V; -std::vector xnew; -xnew.push_back( trans.x);xnew.push_back( trans.y); -V.push_back(xnew); - -points.points.clear(); -pub.publish(points) ; - - - - - - - -std::vector frontiers; -int i=0; -float xr,yr; -std::vector x_rand,x_nearest,x_new; - - -// Main loop -while (ros::ok()){ - - -// Sample free -x_rand.clear(); -xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx; -yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty; - - -x_rand.push_back( xr ); x_rand.push_back( yr ); - - -// Nearest -x_nearest=Nearest(V,x_rand); - -// Steer - -x_new=Steer(x_nearest,x_rand,eta); - - -// ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle -char checking=ObstacleFree(x_nearest,x_new,mapData); - - if (checking==-1){ - exploration_goal.header.stamp=ros::Time(0); - exploration_goal.header.frame_id=mapData.header.frame_id; - exploration_goal.point.x=x_new[0]; - exploration_goal.point.y=x_new[1]; - exploration_goal.point.z=0.0; - p.x=x_new[0]; - p.y=x_new[1]; - p.z=0.0; - points.points.push_back(p); - pub.publish(points) ; - targetspub.publish(exploration_goal); - points.points.clear(); - - } - - - else if (checking==1){ - V.push_back(x_new); - - p.x=x_new[0]; - p.y=x_new[1]; - p.z=0.0; - line.points.push_back(p); - p.x=x_nearest[0]; - p.y=x_nearest[1]; - p.z=0.0; - line.points.push_back(p); - - } - - - -pub.publish(line); - - - - -ros::spinOnce(); -rate.sleep(); - }return 0;}