$search
00001 #!/usr/bin/env python 00002 import roslib 00003 roslib.load_manifest('turtlebot_actions') 00004 00005 import rospy 00006 00007 import os 00008 import sys 00009 import time 00010 00011 from turtlebot_actions.msg import * 00012 from actionlib_msgs.msg import * 00013 00014 00015 import actionlib 00016 00017 00018 def main(): 00019 rospy.init_node("find_fiducial_pose_test") 00020 00021 # Construct action ac 00022 rospy.loginfo("Starting action client...") 00023 action_client = actionlib.SimpleActionClient('find_fiducial_pose', FindFiducialAction) 00024 action_client.wait_for_server() 00025 rospy.loginfo("Action client connected to action server.") 00026 00027 # Call the action 00028 rospy.loginfo("Calling the action server...") 00029 action_goal = FindFiducialGoal() 00030 action_goal.camera_name = "/camera/rgb" 00031 action_goal.pattern_width = 7 00032 action_goal.pattern_height = 6 00033 action_goal.pattern_size = 0.027 00034 action_goal.pattern_type = 0 00035 00036 if action_client.send_goal_and_wait(action_goal, rospy.Duration(50.0), rospy.Duration(50.0)) == GoalStatus.SUCCEEDED: 00037 rospy.loginfo('Call to action server succeeded') 00038 else: 00039 rospy.logerr('Call to action server failed') 00040 00041 00042 if __name__ == "__main__": 00043 main()