def __init__(self, *args, **kwargs): print("Initializing Subscriber/Publishers/Actions/Services/Variables") """ Initialize all ROS services, actions and Pub/Sub Service to be called --> rrt_star_planner_service , dynamic_planner_service Subscribed Topics --> /scan /odom Published Topics --> Parameters Set --> """ #rrt_star Planner Service #class variables initialized: self.flag = 0 self.start = Point_xy([0, 0]) self.goal = Point_xy([0, 0]) self.final_dest = [0, 0] self.scan_list = [] self.current_pos = Pose() self.currentgps = NavSatFix() self.deviation = False nan = 100 # self.scan_list =[1.309682011604309, 1.3101788759231567, 1.310431718826294, 1.3106871843338013, 1.3109458684921265, 1.311471939086914, 1.311739444732666, 1.3120099306106567, 1.3122836351394653, 1.3125602006912231, 1.3128397464752197, 1.3134082555770874, 1.313697099685669, 1.3139890432357788, 1.3142839670181274, 1.3145822286605835, 1.3151880502700806, 1.3154956102371216, 1.3158066272735596, 1.3161206245422363, 1.316437840461731, 1.316758394241333, 1.3174091577529907, 1.317739486694336, 1.3180732727050781, 1.318410038948059, 1.3187503814697266, 1.3190940618515015, 1.3194409608840942, 1.3201451301574707, 1.3205022811889648, 1.3208627700805664, 1.3212268352508545, 1.3215943574905396, 1.3219653367996216, 1.3223397731781006, 1.3230992555618286, 1.3234843015670776, 1.3238729238510132, 1.3242650032043457, 1.3246607780456543, 1.3250601291656494, 1.325463056564331, 1.3258696794509888, 1.3266940116882324, 1.3271117210388184, 1.32753324508667, 1.327958583831787, 1.3283874988555908, 1.3288201093673706, 1.3292566537857056, 1.3296970129013062, 1.3301411867141724, 1.3310409784317017, 1.3314965963363647, 1.331956148147583, 1.332419753074646, 1.3328871726989746, 1.3333585262298584, 1.333833932876587, 1.3343132734298706, 1.3347965478897095, 1.335283875465393, 1.3362706899642944, 1.3367700576782227, 1.3372735977172852, 1.3377811908721924, 1.3382928371429443, 1.3388087749481201, 1.3393288850784302, 1.339853048324585, 1.340381383895874, 1.3409141302108765, 1.3414509296417236, 1.3419921398162842, 1.342537522315979, 1.3436414003372192, 1.3441998958587646, 1.3447626829147339, 1.3453298807144165, 1.345901370048523, 1.3464775085449219, 1.347057819366455, 1.3476428985595703, 1.3482322692871094, 1.3488261699676514, 1.3494247198104858, 1.3500275611877441, 1.3506351709365845, 1.3512473106384277, 1.3518640995025635, 1.3524854183197021, 1.3531115055084229, 1.353742241859436, 1.3543776273727417, 1.35566246509552, 1.3563120365142822, 1.356966495513916, 1.3576256036758423, 1.3582897186279297, 1.3589587211608887, 1.3596327304840088, 1.360311508178711, 1.3609951734542847, 1.361683964729309, 1.362377405166626, 1.363076090812683, 1.3637796640396118, 1.3644883632659912, 1.3652020692825317, 1.365920901298523, 1.3666447401046753, 1.3673737049102783, 1.3681076765060425, 1.3688467741012573, 1.3695908784866333, 1.370340347290039, 1.3710949420928955, 1.3718549013137817, 1.3726199865341187, 1.3733903169631958, 1.3741658926010132, 1.3749470710754395, 1.375733494758606, 1.3765252828598022, 1.3773225545883179, 1.3781251907348633, 1.378933310508728, 1.379746913909912, 1.380566120147705, 1.3813908100128174, 1.382220983505249, 1.3830569982528687, 1.3838989734649658, 1.3847463130950928, 1.3855993747711182, 1.386458158493042, 1.3873226642608643, 1.3881930112838745, 1.3890687227249146, 1.3899502754211426, 1.3908374309539795, 1.3917304277420044, 1.3926293849945068, 1.3935341835021973, 1.3944448232650757, 1.3953614234924316, 1.3962838649749756, 1.3972123861312866, 1.3981468677520752, 1.3990874290466309, 1.4000341892242432, 1.4009875059127808, 1.401947021484375, 1.4029126167297363, 1.4038845300674438, 1.404862403869629, 1.4058465957641602, 1.406836986541748, 1.4078336954116821, 1.4088366031646729, 1.4098458290100098, 1.4108614921569824, 1.4118835926055908, 1.4129116535186768, 1.4139459133148193, 1.4149869680404663, nan, 1.41603422164917, 1.4170881509780884, 1.4181488752365112, 1.4192163944244385, 1.4202903509140015, 1.4213711023330688, 1.4224584102630615, 1.4235525131225586, 1.42465341091156, 1.4257612228393555, 1.4268758296966553, 1.4279974699020386, 1.4291259050369263, 1.4302610158920288, 1.4314026832580566, 1.4325510263442993, 1.433706283569336, 1.434868574142456, 1.4360378980636597, 1.4372141361236572, 1.438397765159607, 1.439588189125061, nan, 1.4407858848571777, 1.441990613937378, 1.4432027339935303, 1.4444221258163452, 1.4456491470336914, 1.4468833208084106, 1.4481250047683716, 1.4493738412857056, 1.4506300687789917, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan] self.currentOdom = Odometry() self.ball_detected = False self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_cb, queue_size=10) self.odom_sub = rospy.Subscriber('zed/zed_node/odom', Odometry, self.odom_cb) self.goal_sub = rospy.Subscriber('/goal2', Point_xy, self.goal_cb) self.result_pub = rospy.Publisher('/acknowledge', String, queue_size=10) #self.gps_Sub = rospy.Subscriber('global_position/local' , NavSatFix , self.gps_cb) self.rrt_star_path = rospy.ServiceProxy('rrt_planner', Planner) self.ball_detector = rospy.ServiceProxy('adjust_service', Adjust) self.move_client = actionlib.SimpleActionClient( 'commander', moveToGoalAction) self.move_client.wait_for_server() self.rotate_client = actionlib.SimpleActionClient( 'rotator', RotateToGoalAction) self.rotate_client.wait_for_server() self.rotate_360_client = actionlib.SimpleActionClient( 'rotator_360', Rotate360Action) self.rotate_360_client.wait_for_server() self.dynamic_manager = rospy.ServiceProxy('dynamic_planner_service', Dynamic) rate = rospy.Rate(1) rate.sleep() print( "Done initializing Subscriber/Publishers/Actions/Services/Variables" )
def plan(self , request): """Call Path planner selected(RRT*, ) Args: request: Request by ros client. Contains start_pos: tuple with start point coordinates. end_pos: tuple with end point coordinates. scan_list: list of obstacles which themselves are list of points animation: flag for showing planning visualization (default False) Returns: RETURN_RESP: PlannerResponse() navigation/PointArray path navigation/Point_xy[] points float32[] point bool ack """ rospy.loginfo("Planning Request received") #Converting request from ros msg format -> basic python data type ST_PT = tuple(request.start_pos.point) END_PT = tuple(request.goal_pos.point) SCAN = request.scan_list #OBSTACLE = [] #Extracting Obstacle information from ROBST #for pt_array in ROBST : # tmp = [] # OBSTACLE.append(tmp) RETURN_RESP = PlannerResponse() print("-"*30) rospy.loginfo(" Starting to plan from %r -> %r \n Obstacles-> %r"%(ST_PT,END_PT,SCAN)) print("-"*30) #Make class instance and get path tree = RRTStar(sample_area=(-5, 15), sampler=sampler, expand_dis=0.1) PATH , node_list = tree(ST_PT , END_PT , SCAN) RETURN_RESP.path.points = [Point_xy( [ pts[0] , pts[1] ] ) for pts in PATH] RETURN_RESP.ack = True print("-"*30) return RETURN_RESP
def convert_utm(data): global pub a = data.latitude b = data.longitude c = current_gps.latitude d = current_gps.longitude q, w, e, r = utm.from_latlon(a, b) t, y, u, i = utm.from_latlon(c, d) local_x = t - q local_y = y - w #print(q,w ,"to" ,t,y) #print(t-q,y-w) #print(math.sqrt((t-q)**2+(y-w)**2)) pub.publish(Point_xy(local_to_global_pt([local_x, local_y], pose)))
def global_to_local_goal(self, pt_in_global): x2 = pt_in_global.point[0] y2 = pt_in_global.point[1] x1 = self.currentOdom.pose.pose.position.x y1 = self.currentOdom.pose.pose.position.y y_diff = y2 - y1 x_diff = x2 - x1 #dest_yaw = math.atan2(y_diff,x_diff) orientation_q = self.currentOdom.pose.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, curr_yaw) = euler_from_quaternion(orientation_list) #theta=dest_yaw-curr_yaw #r=math.sqrt(x_diff**2+y_diff**2) x_ret = x_diff * math.cos(curr_yaw) + y_diff * math.sin(curr_yaw) y_ret = -x_diff * math.sin(curr_yaw) + y_diff * math.cos(curr_yaw) return Point_xy([x_ret, y_ret])
def local_to_global_path(self, path_in_local): path_in_global = PointArray() x1 = self.currentOdom.pose.pose.position.x y1 = self.currentOdom.pose.pose.position.y orientation_q = self.currentOdom.pose.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, theta1) = euler_from_quaternion(orientation_list) for i in range(len(path_in_local.points)): x = path_in_local.points[i].point[0] y = path_in_local.points[i].point[1] theta2 = math.atan2(y, x) theta = theta1 + theta2 r = math.sqrt(x**2 + y**2) x2 = r * math.cos(theta) y2 = r * math.sin(theta) path_in_global.points.append(Point_xy([x1 + x2, y1 + y2])) return path_in_global
def plan(self, request): """Call Path planner selected(RRT, ) Args: request: Request by ros client. Contains start , goal , obstacle_list as: navigation/Point_xy start float32[] point navigation/Point_xy goal float32[] point navigation/PolyArray obstacle_list navigation/PointArray[] polygons navigation/Point_xy[] points float32[] point Returns: RETURN_RESP: PlannerResponse() navigation/PointArray path navigation/Point_xy[] points float32[] point bool ack """ rospy.loginfo("Planning Request received") #Converting request from ros msg format -> basic python data type ST_PT = tuple(request.start.point) END_PT = tuple(request.goal.point) ROBST = request.obstacle_list.polygons OBSTACLE = [] #Extracting Obstacle information from ROBST for pt_array in ROBST: tmp = [] tmp = [tuple(pt.point) for pt in pt_array.points] OBSTACLE.append(tmp) RETURN_RESP = PlannerResponse() print("-" * 30) rospy.loginfo(" Starting to plan from %r -> %r \n Obstacles-> %r" % (ST_PT, END_PT, OBSTACLE)) print("-" * 30) #Make class instance and get path,optimized_path tree = RRT(sample_area=(-5, 15), sampler=sampler, expand_dis=0.1) PATH, node_list = tree(ST_PT, END_PT, OBSTACLE) OPTIMIZED_PATH = path_optimizer(PATH, OBSTACLE) #Convert optimized path to Ros Format and Send RETURN_RESP.path.points = [ Point_xy([pts[0], pts[1]]) for pts in OPTIMIZED_PATH ] RETURN_RESP.ack = True print("-" * 30) return RETURN_RESP
def main(self): print("In MAIN") """ Args: Attributes: sets self.currentOdom Return: """ # rospy.set_param('start_location',"["+str(self.currentgps.latitude)+","+str(self.currentgps.longitude)+"]") # while not rospy.is_shutdown: while (self.flag): final_path = [] # self.goal = rospy.get_param('') try: print("Calculating RRT-PATH") local_goal = self.global_to_local_goal(self.goal) print("LOCAL GOAL", local_goal) response = self.rrt_star_path(Point_xy([0, 0]), local_goal, self.scan_list) if response.ack: local_path = response.path print("local path", local_path) final_path = self.local_to_global_path(local_path) print("path calculated", final_path) else: print("Path not found! Retrying!") continue except rospy.ServiceException, e: print "Service call failed: %s" % e while len(final_path.points) != 1: #rotate towards goal print("Rotating the bot") goal2 = RotateToGoalGoal() goal2.goal = final_path.points[-2] self.rotate_client.send_goal(goal2, feedback_cb=self.rotator_fb) self.rotate_client.wait_for_result() print("Done rotating the bot") # rospy.loginfo("Asking the bot to move.") print("Calling dynamic manager service") x1 = final_path.points[-1].point[0] y1 = final_path.points[-1].point[1] x2 = final_path.points[-2].point[0] y2 = final_path.points[-2].point[1] dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) response_1 = self.dynamic_manager(Point_xy([0, 0]), Point_xy([dist, 0]), self.scan_list) #response_1 = self.dynamic_manager(final_path.points[-1],final_path.points[-2],self.scan_list) if response_1.ack: print("No intersection!!!!") #call commander to move the bot print("Moving the bot") goal1 = moveToGoalGoal() goal1.goal = final_path.points[-2] self.move_client.send_goal(goal1, feedback_cb=self.commander_fb) # rospy.loginfo("Asking the bot to move.") self.move_client.wait_for_result( rospy.Duration.from_sec(100.0)) print("Bot has moved", self.currentOdom.pose.pose.position.x, self.currentOdom.pose.pose.position.y) #remove first node when successful print("Final path updated") final_path.points = final_path.points[:-1] else: # try: # print("Recalculating RRT-Path") # response = self.rrt_star_path(final_path.points[0],self.goal,self.scan_list) # if response.ack: # print("Path Recalculated") # final_path = response.path # else: # continue # except rospy.ServiceException, e: # print "Service call failed: %s"%e break print("updated final_path", final_path.points) if (len(final_path.points) == 1): self.flag = 0
#! /usr/bin/env python import roslib roslib.load_manifest('navigation') import rospy import actionlib from navigation.msg import moveToGoalAction, moveToGoalGoal, Point_xy def feedback_cb(feedback): print('[Feedback] Distance left: %f' % (feedback.distance_left)) if __name__ == '__main__': rospy.init_node('commander_client') client = actionlib.SimpleActionClient('commander', moveToGoalAction) client.wait_for_server() print("Done waiting for server") goal1 = moveToGoalGoal() goal1.goal = Point_xy([1.2, 0]) # Fill in the goal here client.send_goal(goal1, feedback_cb=feedback_cb) # print("Goal sent"+goal1) client.wait_for_result(rospy.Duration.from_sec(500.0)) print("Done waiting for result") print('[Result] State: %d' % (client.get_state())) print('[Result] Status: %s' % (client.get_goal_status_text())) print('[Result] Goal Reached?: %r' % (client.get_result().result))
#! /usr/bin/env python import roslib roslib.load_manifest('navigation') import rospy import actionlib from navigation.msg import RotateToGoalAction, RotateToGoalGoal, Point_xy def feedback_cb(feedback): print('[Feedback] Time elapsed: %f' % (feedback.angle_left)) if __name__ == '__main__': rospy.init_node('rotator_client') client = actionlib.SimpleActionClient('rotator', RotateToGoalAction) client.wait_for_server() print("Done waiting for server") goal1 = RotateToGoalGoal() goal1.goal = Point_xy([2, 2]) # Fill in the goal here client.send_goal(goal1, feedback_cb=feedback_cb) # print("Goal sent"+goal1) client.wait_for_result(rospy.Duration.from_sec(500.0)) print("Done waiting for result") print('[Result] State: %d' % (client.get_state())) print('[Result] Status: %s' % (client.get_goal_status_text())) print('[Result] Goal Reached?: %r' % (client.get_result().result))
class RootClient(object): """ Root class for Controller Client Attributes: Services Available for this Client: ~ rrt_star_planner_service : navigation/start navigation/goal sensor_msgs/LaserScan | navigation/PointArray bool ~ dynamic_planner_service : navigation/start navigation/goal sensor_msgs/LaserScan | bool ~ commander : navigation/current_pt navigation/next_pt | bool | float64(Distance left to travel) """ def __init__(self, *args, **kwargs): print("Initializing Subscriber/Publishers/Actions/Services/Variables") """ Initialize all ROS services, actions and Pub/Sub Service to be called --> rrt_star_planner_service , dynamic_planner_service Subscribed Topics --> /scan /odom Published Topics --> Parameters Set --> """ #rrt_star Planner Service #class variables initialized: self.flag = 0 self.start = Point_xy([0, 0]) self.goal = Point_xy([0, 0]) self.final_dest = [0, 0] self.scan_list = [] self.current_pos = Pose() self.currentgps = NavSatFix() self.deviation = False nan = 100 # self.scan_list =[1.309682011604309, 1.3101788759231567, 1.310431718826294, 1.3106871843338013, 1.3109458684921265, 1.311471939086914, 1.311739444732666, 1.3120099306106567, 1.3122836351394653, 1.3125602006912231, 1.3128397464752197, 1.3134082555770874, 1.313697099685669, 1.3139890432357788, 1.3142839670181274, 1.3145822286605835, 1.3151880502700806, 1.3154956102371216, 1.3158066272735596, 1.3161206245422363, 1.316437840461731, 1.316758394241333, 1.3174091577529907, 1.317739486694336, 1.3180732727050781, 1.318410038948059, 1.3187503814697266, 1.3190940618515015, 1.3194409608840942, 1.3201451301574707, 1.3205022811889648, 1.3208627700805664, 1.3212268352508545, 1.3215943574905396, 1.3219653367996216, 1.3223397731781006, 1.3230992555618286, 1.3234843015670776, 1.3238729238510132, 1.3242650032043457, 1.3246607780456543, 1.3250601291656494, 1.325463056564331, 1.3258696794509888, 1.3266940116882324, 1.3271117210388184, 1.32753324508667, 1.327958583831787, 1.3283874988555908, 1.3288201093673706, 1.3292566537857056, 1.3296970129013062, 1.3301411867141724, 1.3310409784317017, 1.3314965963363647, 1.331956148147583, 1.332419753074646, 1.3328871726989746, 1.3333585262298584, 1.333833932876587, 1.3343132734298706, 1.3347965478897095, 1.335283875465393, 1.3362706899642944, 1.3367700576782227, 1.3372735977172852, 1.3377811908721924, 1.3382928371429443, 1.3388087749481201, 1.3393288850784302, 1.339853048324585, 1.340381383895874, 1.3409141302108765, 1.3414509296417236, 1.3419921398162842, 1.342537522315979, 1.3436414003372192, 1.3441998958587646, 1.3447626829147339, 1.3453298807144165, 1.345901370048523, 1.3464775085449219, 1.347057819366455, 1.3476428985595703, 1.3482322692871094, 1.3488261699676514, 1.3494247198104858, 1.3500275611877441, 1.3506351709365845, 1.3512473106384277, 1.3518640995025635, 1.3524854183197021, 1.3531115055084229, 1.353742241859436, 1.3543776273727417, 1.35566246509552, 1.3563120365142822, 1.356966495513916, 1.3576256036758423, 1.3582897186279297, 1.3589587211608887, 1.3596327304840088, 1.360311508178711, 1.3609951734542847, 1.361683964729309, 1.362377405166626, 1.363076090812683, 1.3637796640396118, 1.3644883632659912, 1.3652020692825317, 1.365920901298523, 1.3666447401046753, 1.3673737049102783, 1.3681076765060425, 1.3688467741012573, 1.3695908784866333, 1.370340347290039, 1.3710949420928955, 1.3718549013137817, 1.3726199865341187, 1.3733903169631958, 1.3741658926010132, 1.3749470710754395, 1.375733494758606, 1.3765252828598022, 1.3773225545883179, 1.3781251907348633, 1.378933310508728, 1.379746913909912, 1.380566120147705, 1.3813908100128174, 1.382220983505249, 1.3830569982528687, 1.3838989734649658, 1.3847463130950928, 1.3855993747711182, 1.386458158493042, 1.3873226642608643, 1.3881930112838745, 1.3890687227249146, 1.3899502754211426, 1.3908374309539795, 1.3917304277420044, 1.3926293849945068, 1.3935341835021973, 1.3944448232650757, 1.3953614234924316, 1.3962838649749756, 1.3972123861312866, 1.3981468677520752, 1.3990874290466309, 1.4000341892242432, 1.4009875059127808, 1.401947021484375, 1.4029126167297363, 1.4038845300674438, 1.404862403869629, 1.4058465957641602, 1.406836986541748, 1.4078336954116821, 1.4088366031646729, 1.4098458290100098, 1.4108614921569824, 1.4118835926055908, 1.4129116535186768, 1.4139459133148193, 1.4149869680404663, nan, 1.41603422164917, 1.4170881509780884, 1.4181488752365112, 1.4192163944244385, 1.4202903509140015, 1.4213711023330688, 1.4224584102630615, 1.4235525131225586, 1.42465341091156, 1.4257612228393555, 1.4268758296966553, 1.4279974699020386, 1.4291259050369263, 1.4302610158920288, 1.4314026832580566, 1.4325510263442993, 1.433706283569336, 1.434868574142456, 1.4360378980636597, 1.4372141361236572, 1.438397765159607, 1.439588189125061, nan, 1.4407858848571777, 1.441990613937378, 1.4432027339935303, 1.4444221258163452, 1.4456491470336914, 1.4468833208084106, 1.4481250047683716, 1.4493738412857056, 1.4506300687789917, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan] self.currentOdom = Odometry() self.ball_detected = False self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_cb, queue_size=10) self.odom_sub = rospy.Subscriber('zed/zed_node/odom', Odometry, self.odom_cb) self.goal_sub = rospy.Subscriber('/goal2', Point_xy, self.goal_cb) self.result_pub = rospy.Publisher('/acknowledge', String, queue_size=10) #self.gps_Sub = rospy.Subscriber('global_position/local' , NavSatFix , self.gps_cb) self.rrt_star_path = rospy.ServiceProxy('rrt_planner', Planner) self.ball_detector = rospy.ServiceProxy('adjust_service', Adjust) self.move_client = actionlib.SimpleActionClient( 'commander', moveToGoalAction) self.move_client.wait_for_server() self.rotate_client = actionlib.SimpleActionClient( 'rotator', RotateToGoalAction) self.rotate_client.wait_for_server() self.rotate_360_client = actionlib.SimpleActionClient( 'rotator_360', Rotate360Action) self.rotate_360_client.wait_for_server() self.dynamic_manager = rospy.ServiceProxy('dynamic_planner_service', Dynamic) rate = rospy.Rate(1) rate.sleep() print( "Done initializing Subscriber/Publishers/Actions/Services/Variables" ) def scan_cb(self, scan_data): """ Args: scan_data --> sensor_msgs/LaserScan Attributes: sets self.scan_list Return: None """ #print("In Scan_CB") self.scan_list = scan_data.ranges self.scan_list = list(self.scan_list) for i in range(len(self.scan_list)): if (math.isnan(self.scan_list[i])): self.scan_list[i] = 100 self.scan_list = tuple(self.scan_list) # self.scan_list = list(scan_list) # for i in range(len(self.scan_list)): # if(math.isnan(self.scan_list[i])): # #converting nan values to some number # self.scan_list[i] = 1000 # self.scan_list = tuple(self.scan_list) #print("Scan_CB DONE") def odom_cb(self, odom_data): #print("In Odom_CB") """ Args: odom_data --> nav_msgs/Odometry Attributes: sets self.currentOdom Return: None """ self.currentOdom = odom_data #print("Odom_CB DONE") def goal_cb(self, goal_data): """ Args: goal_data --> navigation/NavSatFix Attributes: sets self.goal and self.flag Return: None """ self.flag = 1 self.goal = goal_data #self.start = Point_xy([self.currentOdom.pose.pose.position.x],[currentOdom.pose.pose.position.y]) print("received goal", self.goal) self.main() def gps_cb(self, gps_data): """ Args: gps_data --> sensor_msgs/NavSatFix Attributes: sets self.currentOdom Return: None """ self.currentgps = gps_data def commander_fb(self, fb_data): # print("In Commander_FB") """ Args: feedback from commander Attributes: prints feedback from commander Return: None """ #rospy.loginfo(fb_data.distance_left) # print("Commander_FB DONE") pass def rotator_fb(self, fb_data): # print("In Rotator_FB") """ Args: feedback from commander Attributes: prints feedback from commander Return: None """ #rospy.loginfo(fb_data.angle_left) # print("ROtator_FB DONE")X pass ''' This function brings the goal point with respect to the bot's frame of reference ''' def global_to_local_goal(self, pt_in_global): x2 = pt_in_global.point[0] y2 = pt_in_global.point[1] x1 = self.currentOdom.pose.pose.position.x y1 = self.currentOdom.pose.pose.position.y y_diff = y2 - y1 x_diff = x2 - x1 #dest_yaw = math.atan2(y_diff,x_diff) orientation_q = self.currentOdom.pose.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, curr_yaw) = euler_from_quaternion(orientation_list) #theta=dest_yaw-curr_yaw #r=math.sqrt(x_diff**2+y_diff**2) x_ret = x_diff * math.cos(curr_yaw) + y_diff * math.sin(curr_yaw) y_ret = -x_diff * math.sin(curr_yaw) + y_diff * math.cos(curr_yaw) return Point_xy([x_ret, y_ret]) ''' This function brings the entire path which is in the frame of the bot to the global frame ''' def local_to_global_path(self, path_in_local): path_in_global = PointArray() x1 = self.currentOdom.pose.pose.position.x y1 = self.currentOdom.pose.pose.position.y orientation_q = self.currentOdom.pose.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, theta1) = euler_from_quaternion(orientation_list) for i in range(len(path_in_local.points)): x = path_in_local.points[i].point[0] y = path_in_local.points[i].point[1] theta2 = math.atan2(y, x) theta = theta1 + theta2 r = math.sqrt(x**2 + y**2) x2 = r * math.cos(theta) y2 = r * math.sin(theta) path_in_global.points.append(Point_xy([x1 + x2, y1 + y2])) return path_in_global ''' Here we run the loop till we don't have any more points in the final_path list. After every traversale we remove the previous node from the list. Here after we get the final_path list, we rotate the bot in the next node's orientation and check whether the current node and the next immediate node is free from the obstacles in the obstacle list, if it is then we move the bot towards that node ''' def main(self): print("In MAIN") """ Args: Attributes: sets self.currentOdom Return: """ # rospy.set_param('start_location',"["+str(self.currentgps.latitude)+","+str(self.currentgps.longitude)+"]") # while not rospy.is_shutdown: while (self.flag): final_path = [] # self.goal = rospy.get_param('') try: print("Calculating RRT-PATH") local_goal = self.goal #print("LOCAL GOAL",local_goal) x1 = self.currentOdom.pose.pose.position.x y1 = self.currentOdom.pose.pose.position.y response = self.rrt_star_path(Point_xy([x1, y1]), local_goal, self.scan_list) if response.ack: final_path = response.path #print("local path",local_path) #final_path = self.local_to_global_path(local_path) print("path calculated", final_path) else: print("Path not found! Retrying!") continue except rospy.ServiceException, e: print "Service call failed: %s" % e while len(final_path.points) != 1: #rotate towards goal print("Rotating the bot") goal2 = RotateToGoalGoal() goal2.goal = final_path.points[-2] self.rotate_client.send_goal(goal2, feedback_cb=self.rotator_fb) self.rotate_client.wait_for_result() print("Done rotating the bot") # rospy.loginfo("Asking the bot to move.") print("Calling dynamic manager service") x1 = final_path.points[-1].point[0] y1 = final_path.points[-1].point[1] x2 = final_path.points[-2].point[0] y2 = final_path.points[-2].point[1] dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) response_1 = self.dynamic_manager(Point_xy([0, 0]), Point_xy([dist, 0]), self.scan_list) #response_1 = self.dynamic_manager(final_path.points[-1],final_path.points[-2],self.scan_list) if response_1.ack: print("No intersection!!!!") #call commander to move the bot print("Moving the bot") goal1 = moveToGoalGoal() goal1.goal = final_path.points[-2] goal1.flag = self.deviation self.move_client.send_goal(goal1, feedback_cb=self.commander_fb) # rospy.loginfo("Asking the bot to move.") self.move_client.wait_for_result( rospy.Duration.from_sec(100.0)) if not (self.move_client.get_result().result): self.deviation = True final_path.points.append(final_path.points[-1]) print("Bot is tracking back") continue else: self.deviation = False print("Bot has moved", self.currentOdom.pose.pose.position.x, self.currentOdom.pose.pose.position.y) #remove first node when successful print("Final path updated") final_path.points = final_path.points[:-1] else: # try: # print("Recalculating RRT-Path") # response = self.rrt_star_path(final_path.points[0],self.goal,self.scan_list) # if response.ack: # print("Path Recalculated") # final_path = response.path # else: # continue # except rospy.ServiceException, e: # print "Service call failed: %s"%e break print("updated final_path", final_path.points) if (len(final_path.points) == 1): self.flag = 0 print("************************GOAL REACHED************************") self.result_pub.publish("GOAL REACHED") goal2 = Rotate360Goal() goal2.goal = True print("rotating 360") self.rotate_360_client.send_goal(goal2, feedback_cb=self.rotator_fb) self.rotate_360_client.wait_for_result(rospy.Duration.from_sec(100.0)) if not self.rotate_360_client.get_result().result: print("*****BALL DETECTED**********") self.ball_detector(True) self.ball_detected = True self.result_pub.publish("BALL DETECTED") #new changes if not self.ball_detected: hexagon_local = [[2, 0], [-1, -1.73], [1, -1.73], [1, -1.73], [1, -1.73]] i = 0 while (len(hexagon_local) != 0): i += 1 global_point = local_to_global_pt(hexagon_local[0], self.currentOdom.pose.pose) self.flag = 1 while (self.flag): final_path = [] # self.goal = rospy.get_param('') try: print("Calculating RRT-PATH") local_goal = Point_xy(global_point) #print("LOCAL GOAL",local_goal) x1 = self.currentOdom.pose.pose.position.x y1 = self.currentOdom.pose.pose.position.y response = self.rrt_star_path(Point_xy([x1, y1]), local_goal, self.scan_list) if response.ack: final_path = response.path #print("local path",local_path) #final_path = self.local_to_global_path(local_path) print("path calculated", final_path) else: print("Path not found! Retrying!") continue except rospy.ServiceException, e: print "Service call failed: %s" % e while len(final_path.points) != 1: #rotate towards goal print("Rotating the bot") goal2 = RotateToGoalGoal() goal2.goal = final_path.points[-2] self.rotate_client.send_goal( goal2, feedback_cb=self.rotator_fb) self.rotate_client.wait_for_result() print("Done rotating the bot") # rospy.loginfo("Asking the bot to move.") print("Calling dynamic manager service") x1 = final_path.points[-1].point[0] y1 = final_path.points[-1].point[1] x2 = final_path.points[-2].point[0] y2 = final_path.points[-2].point[1] dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) response_1 = self.dynamic_manager( Point_xy([0, 0]), Point_xy([dist, 0]), self.scan_list) #response_1 = self.dynamic_manager(final_path.points[-1],final_path.points[-2],self.scan_list) if response_1.ack: print("No intersection!!!!") #call commander to move the bot print("Moving the bot") goal1 = moveToGoalGoal() goal1.goal = final_path.points[-2] goal1.flag = self.deviation self.move_client.send_goal( goal1, feedback_cb=self.commander_fb) # rospy.loginfo("Asking the bot to move.") self.move_client.wait_for_result( rospy.Duration.from_sec(100.0)) if not (self.move_client.get_result().result): self.deviation = True final_path.points.append(final_path.points[-1]) print("Bot is tracking back") continue else: self.deviation = False print("Bot has moved", self.currentOdom.pose.pose.position.x, self.currentOdom.pose.pose.position.y) #remove first node when successful print("Final path updated") final_path.points = final_path.points[:-1] else: # try: # print("Recalculating RRT-Path") # response = self.rrt_star_path(final_path.points[0],self.goal,self.scan_list) # if response.ack: # print("Path Recalculated") # final_path = response.path # else: # continue # except rospy.ServiceException, e: # print "Service call failed: %s"%e break print("updated final_path", final_path.points) if (len(final_path.points) == 1): self.flag = 0 print( "***************Reached Goal (Hexagon:%d)***************", i) pub.publish() #rotate 360 print("rotating 360") goal2 = Rotate360Goal() goal2.goal = True self.rotate_360_client.send_goal( goal2, feedback_cb=self.rotator_fb) # rospy.loginfo("Asking the bot to move.") self.rotate_360_client.wait_for_result( rospy.Duration.from_sec(100.0)) if not self.rotate_360_client.get_result().result: print("********BALL DETECTED********") self.result_pub.publish("BALL DETECTED") self.ball_detector(True) break hexagon_local = hexagon_local[1:]