cli_controll.py
Go to the documentation of this file.
1 import rospy
2 import smach
3 import smach_ros
4 import types
5 import tf
6 import math
7 import sys, getopt
8 import subprocess
9 
10 from actionlib import *
11 from actionlib.msg import *
12 from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
13 from asr_flir_ptu_controller.msg import PTUMovementGoal, PTUMovementAction
14 
15 
16 def main(argv):
17  rospy.init_node("ptu_manual_controller_script")
18 
19  rospy.loginfo('Trying to communicate with ptu via action msg.')
20  client = actionlib.SimpleActionClient('ptu_controller_actionlib',
21  PTUMovementAction)
22  if not client.wait_for_server(rospy.Duration.from_sec(10)):
23  rospy.logwarn("Could not connect to ptu action server")
24  return 'aborted'
25 
26  #path to visualization script, which is called after each ptu_movement
27 
28  visualization_script = ''
29  try:
30  opts, args = getopt.getopt(argv,"v:",["visualization="])
31  for opt, arg in opts:
32  if opt in ("-v", "--visualization"):
33  visualization_script = arg
34  print 'Using ' + visualization_script + ' for visualization.'
35  except getopt.GetoptError:
36  print 'No visualization script specified.'
37 
38  while True:
39  pan = float(raw_input("Pan? "))
40  tilt = float(raw_input("Tilt? "))
41  ptu_goal = PTUMovementGoal()
42  ptu_goal.target_joint.header.seq = 0
43  ptu_goal.target_joint.name = ['pan', 'tilt']
44  ptu_goal.target_joint.velocity = [0, 0]
45  ptu_goal.target_joint.position = [pan, tilt]
46  client.send_goal(ptu_goal)
47  if visualization_script != '':
48  subprocess.call(" python " + visualization_script + " 1", shell=True)
49 
50  rospy.spin()
51 
52 if __name__ == '__main__':
53  try:
54  main(sys.argv[1:])
55  except rospy.ROSInterruptException: pass
56 
def main(argv)
Definition: cli_controll.py:16


asr_flir_ptu_controller
Author(s): Ralph Schleicher, Patrick Schlosser
autogenerated on Sun Nov 24 2019 03:28:47