srs_symbolic_grounding: srs_symbolic_grounding: scan_base_pose

00001 #!/usr/bin/env python
00002 #################################################################
00003 ##\file
00004 #
00005 # \note
00006 # Copyright (c) 2012 \n
00007 # University of Bedfordshire \n\n
00008 #
00009 #################################################################
00010 #
00011 # \note
00012 # Project name: care-o-bot
00013 # \note
00014 # ROS stack name: srs_public
00015 # \note
00016 # ROS package name: srs_symbolic_grounding
00017 #
00018 # \author
00019 # Author: Beisheng Liu, email:beisheng.liu@beds.ac.uk
00020 # \author
00021 # Supervised by: Dayou Li, email:dayou.li@beds.ac.uk
00022 #
00023 # \date Date of creation: Mar 2012
00024 #
00025 # \brief
00026 # Grounding scan object surface commands
00027 #
00028 #################################################################
00029 #
00030 # Redistribution and use in source and binary forms, with or without
00031 # modification, are permitted provided that the following conditions are met:
00032 #
00033 # - Redistributions of source code must retain the above copyright
00034 # notice, this list of conditions and the following disclaimer. \n
00035 # - Redistributions in binary form must reproduce the above copyright
00036 # notice, this list of conditions and the following disclaimer in the
00037 # documentation and/or other materials provided with the distribution. \n
00038 # - Neither the name of the University of Bedfordshire nor the names of its
00039 # contributors may be used to endorse or promote products derived from
00040 # this software without specific prior written permission. \n
00041 #
00042 # This program is free software: you can redistribute it and/or modify
00043 # it under the terms of the GNU Lesser General Public License LGPL as
00044 # published by the Free Software Foundation, either version 3 of the
00045 # License, or (at your option) any later version.
00046 #
00047 # This program is distributed in the hope that it will be useful,
00048 # but WITHOUT ANY WARRANTY; without even the implied warranty of
00049 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00050 # GNU Lesser General Public License LGPL for more details.
00051 #
00052 # You should have received a copy of the GNU Lesser General Public
00053 # License LGPL along with this program.
00054 # If not, see <http://www.gnu.org/licenses/>.
00055 #
00056 #################################################################
00057 
00058 import roslib; roslib.load_manifest('srs_symbolic_grounding')
00059 from srs_symbolic_grounding.srv import *
00060 #from srs_symbolic_grounding.msg import *
00061 from std_msgs.msg import *
00062 from geometry_msgs.msg import *
00063 from nav_msgs.msg import *
00064 from nav_msgs.srv import *
00065 #from srs_knowledge.msg import *
00066 import rospy
00067 import math
00068 import tf
00069 from tf.transformations import euler_from_quaternion
00070 
00071 
00072 def getWorkspaceOnMap():
00073         print 'test get all workspace (furnitures basically here) from map'
00074         try:
00075                 requestNewTask = rospy.ServiceProxy('get_workspace_on_map', GetWorkspaceOnMap)
00076                 res = requestNewTask('ipa-kitchen-map', True)
00077                 return res
00078         except rospy.ServiceException, e:
00079                 print "Service call failed: %s"%e
00080 
00081 
00082 def getMapClient(): #read map from navigation service
00083 
00084         try:
00085                 reqMap = rospy.ServiceProxy('static_map', GetMap)       
00086                 res = reqMap()
00087                 return res
00088         except rospy.ServiceException, e:
00089                 print "Service call failed: %s"%e
00090 
00091 
00092 
00093 def obstacleCheck(sbpl, fgl): 
00094         obstacle_checked_scan_base_pose_list = list() #used to save obstacle checked poses
00095         wall_checked_scan_base_pose_list = list() #used to save wall checked poses
00096         all_checked_scan_base_pose_list = list()
00097         scan_base_pose_list = sbpl #read inputs
00098         furniture_geometry_list = fgl
00099 
00100         #obstacle check
00101         dist_to_obstacles = 0.5  #set the minimum distance to the household furnitures
00102         #rospy.loginfo(math.atan(-0.5))
00103         #check all of the poses from the scan pose list with all the household furnitures to find a obstacle free scan pose list. the poses will be stored in obstacle_checked_scan_base_pose_list.
00104         index_1 = 0
00105         while index_1 < len(scan_base_pose_list):
00106                 index_2 = 0
00107                 while index_2 < len(furniture_geometry_list):
00108                         th = math.atan((scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) / (scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x))
00109                         if scan_base_pose_list[index_1].x < furniture_geometry_list[index_2].pose.x and scan_base_pose_list[index_1].y > furniture_geometry_list[index_2].pose.y:
00110                                 th = math.pi + th
00111                         if scan_base_pose_list[index_1].x < furniture_geometry_list[index_2].pose.x and scan_base_pose_list[index_1].y < furniture_geometry_list[index_2].pose.y:
00112                                 th = -math.pi + th
00113                         delta_x = math.sqrt((scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.cos(th - furniture_geometry_list[index_2].pose.theta)
00114                         delta_y = math.sqrt((scan_base_pose_list[index_1].x - furniture_geometry_list[index_2].pose.x) ** 2 + (scan_base_pose_list[index_1].y - furniture_geometry_list[index_2].pose.y) ** 2) * math.sin(th - furniture_geometry_list[index_2].pose.theta)
00115                         if (delta_x <= -(furniture_geometry_list[index_2].w / 2.0 + dist_to_obstacles) or delta_x >= (furniture_geometry_list[index_2].w / 2.0 + dist_to_obstacles)) or (delta_y <= -(furniture_geometry_list[index_2].l / 2.0 + dist_to_obstacles) or delta_y >= (furniture_geometry_list[index_2].l / 2.0 + dist_to_obstacles)):
00116                                 index_2 += 1
00117                         else:
00118                                 break
00119                 if index_2 == len(furniture_geometry_list):
00120                         obstacle_checked_scan_base_pose_list.append(scan_base_pose_list[index_1])
00121                 index_1 += 1
00122         #rospy.loginfo(obstacle_checked_scan_base_pose_list)
00123 
00124         if obstacle_checked_scan_base_pose_list: #check if there is a obstacle free pose in the list.
00125                         
00126                 #wall check
00127                 data = getMapClient() #get occupancy grid map from the navigation serves 
00128 
00129                 
00130                 dist_to_walls = 0.5 #set the minimum distance to the walls
00131                 threshold = 10.0 #set the threshold to decide if a pose is occupaied. >threshold:occupied.
00132                 step_angle = 5.0 #set the step angle for putting points around the robot for the wall check (360 / step_angle points will be used)
00133 
00134                 #check all of the poses from the obstacle_checked_scan_base_pose_list with the occupancy map to find a wall free scan pose list
00135                 index_3 = 0
00136                 while index_3 < len(obstacle_checked_scan_base_pose_list):
00137                         map_index_list = list()
00138                         n = 0
00139                         while n < int(360.0 / step_angle):
00140                                 wall_check_point_x = obstacle_checked_scan_base_pose_list[index_3].x + dist_to_walls * math.cos(n * step_angle / 180.0 * math.pi)
00141                                 wall_check_point_y = obstacle_checked_scan_base_pose_list[index_3].y + dist_to_walls * math.sin(n * step_angle / 180.0 * math.pi)
00142                                 
00143                                 #rospy.loginfo([wall_check_point_x, wall_check_point_y])
00144 
00145                                 map_index = int((wall_check_point_y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((wall_check_point_x - data.map.info.origin.position.x) / data.map.info.resolution)
00146                                 map_index_list.append(map_index)
00147                                 n += 1
00148                                 
00149                         map_index = int((obstacle_checked_scan_base_pose_list[index_3].y - data.map.info.origin.position.y) / data.map.info.resolution) * data.map.info.width + int((obstacle_checked_scan_base_pose_list[index_3].x - data.map.info.origin.position.x) / data.map.info.resolution)
00150                         map_index_list.append(map_index)
00151                         #rospy.loginfo(map_index_list)
00152 
00153                         index_4 = 0
00154                         while index_4 < len(map_index_list):
00155                                 #rospy.loginfo(data.map.data[map_index_list[index_4]])
00156                                 if -1 < data.map.data[map_index_list[index_4]] < threshold:
00157                                         index_4 += 1
00158                                 else:
00159                                         break
00160                         if index_4 == len(map_index_list):
00161                                 wall_checked_scan_base_pose_list.append(obstacle_checked_scan_base_pose_list[index_3])
00162                         index_3 += 1
00163         
00164         return  wall_checked_scan_base_pose_list                        
00165         #rospy.loginfo(wall_checked_scan_base_pose_list)
00166 
00167 
00168 
00169 #calculate scan base poses
00170 def handle_scan_base_pose(req):
00171 
00172         
00173 
00174         
00175 
00176                 
00177         #get furniture information from knowledge base
00178         workspace_info = getWorkspaceOnMap()    
00179         furniture_geometry_list = list()
00180         furniture_geometry_list = workspace_info.objectsInfo
00181         
00182         
00183         #transfrom list
00184         index = 0
00185         furniture_geometry_list = list()
00186         while index < len(furniture_geometry_list):
00187                 furniture_geometry = FurnitureGeometry()
00188                 furniture_geometry.pose.x = furniture_geometry_list[index].pose.position.x
00189                 furniture_geometry.pose.y = furniture_geometry_list[index].pose.position.y
00190                 furniture_pose_rpy = tf.transformations.euler_from_quaternion([furniture_geometry_list[index].pose.orientation.x, furniture_geometry_list[index].pose.orientation.y, furniture_geometry_list[index].pose.orientation.z, furniture_geometry_list[index].pose.orientation.w])             
00191                 furniture_geometry.pose.theta = furniture_pose_rpy[2]
00192                 furniture_geometry.l = furniture_geometry_list[index].l
00193                 furniture_geometry.w = furniture_geometry_list[index].w
00194                 furniture_geometry.h = furniture_geometry_list[index].h
00195                 furniture_geometry_list.append(furniture_geometry)
00196                 index += 1
00197 
00198 
00199         
00200         
00201 
00202         #transform from knowledge base data to function useable data    
00203         parent_obj_x = req.parent_obj_geometry.pose.position.x
00204         parent_obj_y = req.parent_obj_geometry.pose.position.y
00205         parent_obj_rpy = tf.transformations.euler_from_quaternion([req.parent_obj_geometry.pose.orientation.x, req.parent_obj_geometry.pose.orientation.y, req.parent_obj_geometry.pose.orientation.z, req.parent_obj_geometry.pose.orientation.w])
00206         parent_obj_th = parent_obj_rpy[2]
00207         parent_obj_l = req.parent_obj_geometry.l
00208         parent_obj_w = req.parent_obj_geometry.w
00209         parent_obj_h = req.parent_obj_geometry.h
00210 
00211         #rpy = tf.transformations.euler_from_quaternion([0, 0, -0.68, 0.73])
00212         #rospy.loginfo(rpy[2])
00213 
00214         #rospy.loginfo(req.parent_obj_geometry)
00215 
00216         
00217         
00218         
00219         
00220 
00221         #calculate the detection width
00222         rb_distance = 0.7 #distance between the robot base and the edge of the parent obj
00223         robot_h = 1.4 #set the height of the detector
00224         detection_angle = (30.0 / 180.0) * math.pi #set the detection angle (wide) of the detector 
00225         camera_distance = math.sqrt((robot_h - parent_obj_h) ** 2 + (rb_distance - 0.2) ** 2) #distance between the detector and the surface of the parent obj
00226         detection_w = 2 * (camera_distance * math.tan(0.5 * detection_angle)) #detection wide
00227         #rospy.loginfo(detection_w)
00228 
00229 
00230 
00231 
00232 
00233 
00234         
00235         #variable structure define
00236         scan_base_pose_1 = Pose2D()
00237         scan_base_pose_2 = Pose2D()
00238         scan_base_pose_3 = Pose2D()
00239         scan_base_pose_4 = Pose2D()
00240         
00241         #create lists to store scan base poses
00242         scan_base_pose_list_1 = list()
00243         scan_base_pose_list_2 = list()
00244         scan_base_pose_list_3 = list()
00245         scan_base_pose_list_4 = list()
00246 
00247         #fix angle problem
00248         if parent_obj_th < 0:
00249                 parent_obj_th += 2.0 * math.pi
00250 
00251         elif parent_obj_th > 2.0 * math.pi:
00252                 parent_obj_th -= 2.0 * math.pi
00253         #rospy.loginfo(parent_obj_th)
00254 
00255 
00256         #to decide which side of the table is facing the robot 
00257         if ((parent_obj_th >= 0) & (parent_obj_th <= (45.0 / 180.0 * math.pi))) | ((parent_obj_th >= (135.0 / 180.0 * math.pi)) & (parent_obj_th <= (225.0 / 180.0 * math.pi))) | ((parent_obj_th >= (315.0 / 180.0 * math.pi)) & (parent_obj_th < 360)):
00258                 
00259                 #calculate 4 lists of scan poses, each list is for scanning from one side of the table.
00260                 for num in range(int((parent_obj_l / detection_w) + 0.99)):
00261                         scan_base_pose_1 = Pose2D()
00262                         scan_base_pose_1.x = parent_obj_x - (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) - (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00263                         scan_base_pose_1.y = parent_obj_y - (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) + (0.5 * parent_obj_l - 0.5 *  detection_w - num * detection_w) * math.cos(parent_obj_th)
00264                         scan_base_pose_1.theta = parent_obj_th + math.pi
00265                         if scan_base_pose_1.theta > math.pi:
00266                                 scan_base_pose_1.theta -= 2.0 * math.pi
00267                         elif scan_base_pose_1.theta < -math.pi:
00268                                 scan_base_pose_1.theta += 2.0 * math.pi
00269                         scan_base_pose_list_1.append(scan_base_pose_1)
00270 
00271                 #rospy.loginfo(scan_base_pose_list_1)
00272                 
00273                                 
00274                 for num in range(int((parent_obj_l / detection_w) + 0.99)):
00275                         scan_base_pose_2 = Pose2D()
00276                         scan_base_pose_2.x = parent_obj_x + (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) + (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00277                         scan_base_pose_2.y = parent_obj_y + (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) - (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00278                         scan_base_pose_2.theta = parent_obj_th
00279                         if scan_base_pose_2.theta > math.pi:
00280                                 scan_base_pose_2.theta -= 2.0 * math.pi
00281                         elif scan_base_pose_2.theta < -math.pi:
00282                                 scan_base_pose_2.theta += 2.0 * math.pi
00283                         scan_base_pose_list_2.append(scan_base_pose_2)
00284 
00285 
00286 
00287 
00288                 for num in range(int((parent_obj_w / detection_w) + 0.99)):
00289 
00290                         scan_base_pose_3 = Pose2D()
00291                         scan_base_pose_3.x = parent_obj_x + (parent_obj_l * 0.5 + rb_distance) * math.sin(parent_obj_th) - (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00292                         scan_base_pose_3.y = parent_obj_y - (parent_obj_l * 0.5 + rb_distance) * math.cos(parent_obj_th) - (0.5 * parent_obj_w - 0.5 *  detection_w - num * detection_w) * math.sin(parent_obj_th)
00293                         scan_base_pose_3.theta = parent_obj_th - 0.5 * math.pi
00294                         if scan_base_pose_3.theta > math.pi:
00295                                 scan_base_pose_3.theta -= 2.0 * math.pi
00296                         elif scan_base_pose_3.theta < -math.pi:
00297                                 scan_base_pose_3.theta += 2.0 * math.pi
00298                         scan_base_pose_list_3.append(scan_base_pose_3)
00299 
00300                 
00301 
00302 
00303                 for num in range(int((parent_obj_w / detection_w) + 0.99)):
00304                         scan_base_pose_4 = Pose2D()
00305                         scan_base_pose_4.x = parent_obj_x - (parent_obj_l * 0.5 + rb_distance) * math.sin(parent_obj_th) + (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00306                         scan_base_pose_4.y = parent_obj_y + (parent_obj_l * 0.5 + rb_distance) * math.cos(parent_obj_th) + (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00307                         scan_base_pose_4.theta = parent_obj_th + 0.5 * math.pi
00308                         if scan_base_pose_4.theta > math.pi:
00309                                 scan_base_pose_4.theta -= 2.0 * math.pi
00310                         elif scan_base_pose_4.theta < -math.pi:
00311                                 scan_base_pose_4.theta += 2.0 * math.pi
00312                         scan_base_pose_list_4.append(scan_base_pose_4)
00313 
00314         #the short side is facing the robot     
00315         else:
00316                 parent_obj_th -= 0.5 * math.pi
00317 
00318                 for num in range(int((parent_obj_w / detection_w) + 0.99)):
00319 
00320                         scan_base_pose_1 = Pose2D()
00321                         scan_base_pose_1.x = parent_obj_x - (parent_obj_l * 0.5 + rb_distance) * math.cos(parent_obj_th) - (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00322                         scan_base_pose_1.y = parent_obj_y - (parent_obj_l * 0.5 + rb_distance) * math.sin(parent_obj_th) + (0.5 * parent_obj_w - 0.5 *  detection_w - num * detection_w) * math.cos(parent_obj_th)
00323                         scan_base_pose_1.theta = parent_obj_th + math.pi
00324                         if scan_base_pose_1.theta > math.pi:
00325                                 scan_base_pose_1.theta -= 2.0 * math.pi
00326                         elif scan_base_pose_1.theta < -math.pi:
00327                                 scan_base_pose_1.theta += 2.0 * math.pi
00328                         scan_base_pose_list_1.append(scan_base_pose_1)
00329 
00330 
00331                 
00332                                 
00333                 for num in range(int((parent_obj_w / detection_w) + 0.99)):
00334                         scan_base_pose_2 = Pose2D()
00335                         scan_base_pose_2.x = parent_obj_x + (parent_obj_l * 0.5 + rb_distance) * math.cos(parent_obj_th) + (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00336                         scan_base_pose_2.y = parent_obj_y + (parent_obj_l * 0.5 + rb_distance) * math.sin(parent_obj_th) - (0.5 * parent_obj_w - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00337                         scan_base_pose_2.theta = parent_obj_th
00338                         if scan_base_pose_2.theta > math.pi:
00339                                 scan_base_pose_2.theta -= 2.0 * math.pi
00340                         elif scan_base_pose_2.theta < -math.pi:
00341                                 scan_base_pose_2.theta += 2.0 * math.pi
00342                         scan_base_pose_list_2.append(scan_base_pose_2)
00343 
00344 
00345                 
00346 
00347                 for num in range(int((parent_obj_l / detection_w) + 0.99)):
00348 
00349                         scan_base_pose_3 = Pose2D()
00350                         scan_base_pose_3.x = parent_obj_x + (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) - (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00351                         scan_base_pose_3.y = parent_obj_y - (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) - (0.5 * parent_obj_l - 0.5 *  detection_w - num * detection_w) * math.sin(parent_obj_th)
00352                         scan_base_pose_3.theta = parent_obj_th - 0.5 * math.pi
00353                         if scan_base_pose_1.theta > math.pi:
00354                                 scan_base_pose_1.theta -= 2.0 * math.pi
00355                         elif scan_base_pose_1.theta < -math.pi:
00356                                 scan_base_pose_1.theta += 2.0 * math.pi
00357                         scan_base_pose_list_3.append(scan_base_pose_3)
00358 
00359                 
00360 
00361                                 
00362                 for num in range(int((parent_obj_l / detection_w) + 0.99)):
00363                         scan_base_pose_4 = Pose2D()
00364                         scan_base_pose_4.x = parent_obj_x - (parent_obj_w * 0.5 + rb_distance) * math.sin(parent_obj_th) + (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.cos(parent_obj_th)
00365                         scan_base_pose_4.y = parent_obj_y + (parent_obj_w * 0.5 + rb_distance) * math.cos(parent_obj_th) + (0.5 * parent_obj_l - 0.5 * detection_w - num * detection_w) * math.sin(parent_obj_th)
00366                         scan_base_pose_4.theta = parent_obj_th + 0.5 * math.pi
00367                         if scan_base_pose_1.theta > math.pi:
00368                                 scan_base_pose_1.theta -= 2.0 * math.pi
00369                         elif scan_base_pose_1.theta < -math.pi:
00370                                 scan_base_pose_1.theta += 2.0 * math.pi
00371                         scan_base_pose_list_4.append(scan_base_pose_4)
00372                 
00373         #rospy.loginfo(scan_base_pose_list_1)
00374         #rospy.loginfo(scan_base_pose_list_2)
00375         #rospy.loginfo(scan_base_pose_list_3)
00376         #rospy.loginfo(scan_base_pose_list_4)
00377         
00378         #obstacle check
00379         obstacle_checked_scan_base_pose_list_1 = obstacleCheck(scan_base_pose_list_1, furniture_geometry_list)
00380         obstacle_checked_scan_base_pose_list_2 = obstacleCheck(scan_base_pose_list_2, furniture_geometry_list)
00381         obstacle_checked_scan_base_pose_list_3 = obstacleCheck(scan_base_pose_list_3, furniture_geometry_list)
00382         obstacle_checked_scan_base_pose_list_4 = obstacleCheck(scan_base_pose_list_4, furniture_geometry_list)
00383 
00384 
00385 
00386         #rospy.loginfo([obstacle_checked_scan_base_pose_list_1, obstacle_checked_scan_base_pose_list_2, obstacle_checked_scan_base_pose_list_3, obstacle_checked_scan_base_pose_list_4])
00387         max_len = max(len(obstacle_checked_scan_base_pose_list_1), len(obstacle_checked_scan_base_pose_list_2), len(obstacle_checked_scan_base_pose_list_3), len(obstacle_checked_scan_base_pose_list_4))
00388 
00389         #choose the longest scan pose list 
00390         if len(obstacle_checked_scan_base_pose_list_1) == max_len:
00391                 scan_base_pose_list = obstacle_checked_scan_base_pose_list_1
00392         elif len(obstacle_checked_scan_base_pose_list_2) == max_len:
00393                 scan_base_pose_list = obstacle_checked_scan_base_pose_list_2
00394         elif len(obstacle_checked_scan_base_pose_list_3) == max_len:
00395                 scan_base_pose_list = obstacle_checked_scan_base_pose_list_3
00396         else:
00397                 scan_base_pose_list = obstacle_checked_scan_base_pose_list_4
00398         
00399 
00400         if not scan_base_pose_list:
00401                 print "no valid scan pose."
00402         if max_len == 1:
00403                 th = math.atan((scan_base_pose_list[0].y - parent_obj_y) / (scan_base_pose_list[0].x - parent_obj_x))
00404                 if scan_base_pose_list[0].x < parent_obj_x and scan_base_pose_list[0].y > parent_obj_y:
00405                         th = math.pi + th
00406                 if scan_base_pose_list[0].x < parent_obj_x and scan_base_pose_list[0].y < parent_obj_y:
00407                         th = -math.pi + th
00408                 if th > math.pi:
00409                         th -= 2.0 * math.pi
00410                 elif th < -math.pi:
00411                         th += 2.0 * math.pi
00412                 scan_base_pose_list[0].theta = th
00413         
00414 
00415         return [scan_base_pose_list]
00416 
00417 
00418 
00419 def scan_base_pose_server():
00420         rospy.init_node('scan_base_pose_server')
00421         s = rospy.Service('scan_base_pose', ScanBasePose, handle_scan_base_pose)
00422         print "Ready to receive requests."
00423         rospy.spin()
00424 
00425 
00426 
00427 if __name__ == "__main__":
00428         scan_base_pose_server()
00429 
00430
scan_base_pose_server.py
