cob_map_accessibility_analysis: blocked_goal

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 
00017 
00018 import rospy
00019 
00020 from geometry_msgs.msg import Pose2D
00021 from cob_map_accessibility_analysis.srv import CheckPointAccessibility, CheckPointAccessibilityRequest, CheckPointAccessibilityResponse
00022 from cob_map_accessibility_analysis.srv import CheckPerimeterAccessibility, CheckPerimeterAccessibilityRequest, CheckPerimeterAccessibilityResponse
00023 
00024 
00025 class BlockedGoalAlternative():
00026 
00027     def __init__(self):
00028         rospy.wait_for_service("/map_accessibility_analysis/map_points_accessibility_check")
00029         rospy.wait_for_service("/map_accessibility_analysis/map_perimeter_accessibility_check")
00030 
00031         rospy.loginfo("All map_accessibility_analysis services available")
00032 
00033         self.point_client = rospy.ServiceProxy(
00034             "/map_accessibility_analysis/map_points_accessibility_check", CheckPointAccessibility)
00035         self.point_req = None
00036         self.point_res = None
00037         self.perimeter_client = rospy.ServiceProxy(
00038             "/map_accessibility_analysis/map_perimeter_accessibility_check", CheckPerimeterAccessibility)
00039         self.perimeter_req = None
00040         self.perimeter_res = None
00041 
00042     def check_nav_goal(self, pose2d):
00043         try:
00044             self.point_req = CheckPointAccessibilityRequest()
00045             # array of points which should be checked for accessibility
00046             self.point_req.points_to_check.append(pose2d)
00047             # if true, the path to a goal position must be accessible as well
00048             self.point_req.approach_path_accessibility_check = False
00049 
00050             self.point_res = self.point_client(self.point_req)
00051             # rospy.loginfo("CheckPointAccessibilityResponse")
00052             # print self.point_res
00053         except rospy.ServiceException, e:
00054             rospy.logerr("Service call 'map_points_accessibility_check' failed: %s" % e)
00055             return (False, None)
00056 
00057         for i in range(len(self.point_req.points_to_check)):
00058             if self.point_res.accessibility_flags[i]:
00059                 rospy.loginfo("Nav Goal is valid")
00060                 # print self.point_req.points_to_check[i]
00061                 return (True, self.point_req.points_to_check[i])
00062 
00063         rospy.logwarn("Nav Goal is not accessible")
00064         return (False, None)
00065 
00066     def check_perimeter(self, pose2d):
00067         for radius in [0.1, 0.3, 0.5, 0.7, 1.0, 2.0]:
00068             rospy.loginfo("Checking with perimeter radius %f", radius)
00069             try:
00070                 self.perimeter_req = CheckPerimeterAccessibilityRequest()
00071                 # center of the circle whose perimeter should be checked for accessibility, in [m,m,rad]
00072                 self.perimeter_req.center = pose2d
00073                 self.perimeter_req.radius = radius                   # radius of the circle, in [m]
00074                 # rotational sampling step width for checking points on the perimeter, in [rad]
00075                 self.perimeter_req.rotational_sampling_step = 0.0
00076 
00077                 self.perimeter_res = self.perimeter_client(self.perimeter_req)
00078                 # rospy.loginfo("CheckPerimeterAccessibilityResponse")
00079                 # print self.perimeter_res
00080             except rospy.ServiceException, e:
00081                 rospy.logerr("Service call 'map_perimeter_accessibility_check' failed: %s" % e)
00082                 break
00083 
00084             # use first valid alternative
00085             if not self.perimeter_res.accessible_poses_on_perimeter == []:
00086                 rospy.loginfo("Found valid alternative")
00087                 # print self.perimeter_res.accessible_poses_on_perimeter[0]
00088                 return (True, self.perimeter_res.accessible_poses_on_perimeter[0])
00089 
00090         rospy.logerr("No valid alternative in perimeter")
00091         return (False, None)