$search
00001 #! /usr/bin/env python 00002 00003 import roslib; roslib.load_manifest('pr2_arm_navigation_actions') 00004 import rospy 00005 00006 #move arm messages and action 00007 import actionlib 00008 import pr2_msgs.srv 00009 import arm_navigation_msgs.msg 00010 import arm_navigation_msgs.msg 00011 00012 laser_service_name="/laser_tilt_controller/set_periodic_cmd" 00013 00014 def set_laser_to_slow_scan(): 00015 00016 global laser_service_name 00017 00018 rospy.wait_for_service(laser_service_name) 00019 set_laser_per = rospy.ServiceProxy(laser_service_name,pr2_msgs.srv.SetPeriodicCmd) 00020 00021 laser_per = pr2_msgs.srv.SetPeriodicCmdRequest() 00022 00023 laser_per.command.header.stamp = rospy.Time.now() 00024 laser_per.command.profile = "linear" 00025 laser_per.command.period = 10 00026 laser_per.command.amplitude = .25 00027 laser_per.command.offset = .5 00028 resp1 = set_laser_per(laser_per) 00029 00030 if(resp1): 00031 print "laser cmd successful" 00032 else: 00033 print "something wrong with laser command" 00034 00035 00036 def trigger_static_map(): 00037 00038 static_map_client = actionlib.SimpleActionClient("make_static_collision_map", arm_navigation_msgs.msg.MakeStaticCollisionMapAction) 00039 00040 static_map_client.wait_for_server() 00041 00042 static_map_goal = arm_navigation_msgs.msg.MakeStaticCollisionMapGoal() 00043 static_map_goal.cloud_source = "full_cloud_filtered" 00044 static_map_goal.number_of_clouds = 2 00045 00046 static_map_client.send_goal(static_map_goal) 00047 00048 set_laser_to_slow_scan() 00049 00050 static_map_client.wait_for_result() 00051 00052 print 'Should have taken static map' 00053 00054 if __name__ == '__main__': 00055 # Initializes a rospy node so that the SimpleActionClient can 00056 # publish and subscribe over ROS. 00057 rospy.init_node('take_static_laser_collision_map') 00058 00059 trigger_static_map() 00060 00061 exit()