construct_static_collision_map.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('collider')
00004 roslib.load_manifest('actionlib')
00005 roslib.load_manifest('pr2_controllers_msgs')
00006 roslib.load_manifest('geometry_msgs')
00007 roslib.load_manifest('sensor_msgs')
00008 
00009 import rospy
00010 from actionlib import SimpleActionClient, GoalStatus, SimpleActionServer
00011 from pr2_controllers_msgs.msg import \
00012     PointHeadAction, PointHeadGoal, \
00013     SingleJointPositionAction, SingleJointPositionGoal
00014 from geometry_msgs.msg import PointStamped
00015 from sensor_msgs.msg import PointCloud2
00016 from arm_navigation_msgs.msg import MakeStaticCollisionMapAction, MakeStaticCollisionMapGoal
00017 from collider.msg import ConstructStaticCollisionMapAction
00018 
00019 import numpy as np
00020 
00021 class ConstructStaticCollisionMapServer:
00022 
00023     def __init__(self,
00024                  node_name = 'construct_static_collision_map_server',
00025                  action_name = 'construct_static_collision_map_action',
00026                  head_topic = '/head_traj_controller/point_head_action',
00027                  static_collision_map_topic = '/make_static_collision_map'
00028                  
00029                  ):
00030                  
00031         rospy.init_node(node_name)
00032         rospy.loginfo ( 'waiting for SimpleActionClient: %s'%( head_topic )  )
00033         self.head_client = SimpleActionClient(head_topic,PointHeadAction )
00034         self.head_client.wait_for_server( )
00035 
00036         self.take_static_collision_map_client = SimpleActionClient(static_collision_map_topic, MakeStaticCollisionMapAction)
00037 
00038         self._action_server = SimpleActionServer(action_name, ConstructStaticCollisionMapAction, execute_cb=self.execute_cb)
00039         rospy.loginfo('ready')
00040 
00041     def execute_cb(self, goal):
00042 
00043         self.scan_table()
00044         goal = MakeStaticCollisionMapGoal()
00045         self.take_static_collision_map_client.send_goal(goal)
00046 
00047         self._action_server.set_succeeded()
00048 
00049     def move_head(self, x, y, z,
00050                   min_dur = 0.0,
00051                   max_velocity = 1.0,
00052                   frame_id = 'base_link',
00053                   timeout = 5.0):
00054         point = PointStamped()
00055         point.header.frame_id = frame_id
00056         point.header.stamp = rospy.Time.now()
00057         point.point.x, point.point.y, point.point.z = x, y, z
00058             
00059         goal = PointHeadGoal()
00060         goal.pointing_frame = 'head_plate_frame'
00061         goal.max_velocity = max_velocity
00062         goal.min_duration = rospy.Duration.from_sec( min_dur )
00063         goal.target = point
00064         
00065         self.head_client.send_goal( goal )
00066         self.head_client.wait_for_result( timeout = 
00067                                           rospy.Duration.from_sec( timeout ) )
00068         if not self.head_client.get_state() == GoalStatus.SUCCEEDED:
00069             rospy.logerr( 'can not move head to:\n %s'%( goal ) )
00070             return False
00071             
00072         return True
00073 
00074     def scan_table(self,
00075                    x = 0.5, 
00076                    y_min = -1.0, y_max = 1.0, 
00077                    z = 0.7, 
00078                    steps = 10, 
00079                    min_dur = 1.0, 
00080                    max_wait_time = 10.0
00081                    ):
00082         rospy.loginfo( 'scanning table with x = %5.3f, y_min = %5.3f, y_max = %5.3f, z = %5.3f, steps = %d'%( x, y_min, y_max, z, steps ) )
00083         Y = np.linspace( y_min, y_max, steps )
00084         
00085         for y in Y:
00086         
00087             if not self.move_head( x, y, z, min_dur ):
00088                 return
00089 
00090         rospy.loginfo( 'done scanning table!' )
00091 
00092 if __name__ == '__main__':
00093     ome = ConstructStaticCollisionMapServer()
00094     rospy.spin()


collider
Author(s): Adam Harmat, Gil E. Jones, Kai M. Wurm, Armin Hornung. Maintained by Gil E. Jones
autogenerated on Thu Dec 12 2013 11:07:38