detect_wall_norm.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # Author Melonee Wise
00035 
00036 
00037 import roslib; roslib.load_manifest('pr2_plugs_actions')
00038 import rospy
00039 import actionlib
00040 
00041 from pr2_controllers_msgs.msg import *
00042 from pr2_plugs_msgs.msg import *
00043 from actionlib_msgs.msg import *
00044 from trajectory_msgs.msg import JointTrajectoryPoint
00045 from geometry_msgs.msg import *
00046 import dynamic_reconfigure.client
00047 from stereo_wall_detection.srv import *
00048 
00049 
00050 class DetectWallNormServer:
00051   def __init__(self, name):
00052     self.name = name
00053 
00054     self.head_client = actionlib.SimpleActionClient('head_traj_controller/point_head_action', PointHeadAction)
00055     self.head_client.wait_for_server()
00056 
00057     rospy.wait_for_service('stereo_wall_detection/detect_wall')
00058     rospy.loginfo('detect wall service found')
00059     self.detect_wall_srv = rospy.ServiceProxy('stereo_wall_detection/detect_wall', DetectWall)
00060     self.server = actionlib.simple_action_server.SimpleActionServer(self.name, DetectWallNormAction, self.execute_cb)
00061 
00062     self.projector_client = dynamic_reconfigure.client.Client('camera_synchronizer_node')
00063     self.projector_on = {'projector_mode':3, 'narrow_stereo_trig_mode': 3}
00064     self.projector_off = {'projector_mode':1, 'narrow_stereo_trig_mode': 4}
00065     self.projector_sub = rospy.Subscriber("projector_controller/rising_edge_timestamps", rospy.Header, self.projector_cb)
00066 
00067 
00068 
00069   def execute_cb(self, goal):
00070     rospy.loginfo('detect wall norm')
00071 
00072     #point head at wall
00073     point_head_goal = PointHeadGoal()
00074     point_head_goal.target.header.frame_id = goal.look_point.header.frame_id
00075     point_head_goal.target.point.x = goal.look_point.point.x
00076     point_head_goal.target.point.y = goal.look_point.point.y
00077     point_head_goal.target.point.z = goal.look_point.point.z
00078     point_head_goal.pointing_frame = 'head_tilt_link'
00079     point_head_goal.pointing_axis.x = 1
00080     point_head_goal.pointing_axis.y = 0
00081     point_head_goal.pointing_axis.z = 0
00082 
00083     self.head_client.send_goal(point_head_goal)
00084     self.head_client.wait_for_result(rospy.Duration(20.0))
00085     if self.head_client.get_state() != GoalStatus.SUCCEEDED:
00086       rospy.logerr('%s: Failed to move the head', self.name)
00087       self.server.set_aborted()
00088       return
00089 
00090     # turn on projector
00091     rospy.loginfo('turn on projector')
00092     self.projector_ready = False
00093     while not self.turn_projector_on():
00094       rospy.loginfo('still trying to turn on projector')
00095       rospy.sleep(1.0)
00096     while not self.projector_ready:
00097       rospy.sleep(1.0)
00098       rospy.loginfo('waiting for projector to be ready')
00099     rospy.loginfo('projector truned on')
00100 
00101     # detect wall norm
00102     try:
00103       rospy.loginfo('Call stereo wall detector')
00104       wall = self.detect_wall_srv(DetectWallRequest())
00105       rospy.loginfo('Call stereo wall detector succeeded')
00106     except rospy.ServiceException, e:
00107       rospy.logerr("Service call to wall detector failed")
00108       while not self.turn_projector_off():
00109         rospy.loginfo('still trying to turn off projector')
00110         rospy.sleep(1.0)
00111       self.server.set_aborted()
00112       return
00113 
00114     # turn off projector
00115     rospy.loginfo('turn off projector')
00116     while not self.turn_projector_off():
00117       rospy.loginfo('still trying to turn off projector')
00118       rospy.sleep(1.0)
00119     result = DetectWallNormResult()
00120     result.wall_norm = wall.wall_norm
00121     result.wall_point = wall.wall_point
00122     self.server.set_succeeded(result)
00123 
00124   def projector_cb(self, data):
00125     if(not self.server.is_active()):
00126       self.projector_ready = False
00127       return
00128 
00129     self.projector_ready = True
00130 
00131   def turn_projector_on(self):
00132     try:
00133       self.projector_client.update_configuration(self.projector_on)
00134       return True
00135     except rospy.ServiceException, e:
00136       rospy.logerr('Failed to turn projector on')
00137       return False
00138 
00139   def turn_projector_off(self):
00140     try:
00141       self.projector_client.update_configuration(self.projector_off)
00142       return True
00143     except rospy.ServiceException, e:
00144       rospy.logerr('Failed to turn projector off')
00145       return False
00146 
00147 
00148 
00149 if __name__ == '__main__':
00150 
00151   # Initializes a rospy node so that the SimpleActionClient can
00152   # publish and subscribe over ROS.
00153   name ='detect_wall_norm'
00154   rospy.init_node(name)
00155   detect_wall_norm_server = DetectWallNormServer(name)
00156   rospy.loginfo('%s: Action server running', name)
00157   rospy.spin()


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13