move.py
Go to the documentation of this file.
1 from general_ros import *
2 from configuration import *
3 from asr_msgs.msg import AsrObject, AsrViewport
4 from asr_flir_ptu_controller.msg import PTUMovementGoal, PTUMovementAction, PTUMovementActionResult
5 from move_base_msgs.msg import MoveBaseGoal, MoveBaseAction, MoveBaseActionResult
6 from geometry_msgs.msg import (Pose, PoseWithCovariance, PoseWithCovarianceStamped)
7 from asr_robot_model_services.srv import GetPose
8 from actionlib import *
9 from actionlib.msg import *
10 import subprocess
11 import math
12 import tf
13 import threading
14 import __builtin__
15 
16 # cur ptu position to move ptu by an offset
17 __builtin__.ptuPos = (0, 0) # (pan, tilt)
18 # callback to get cur ptu position
19 def ptuMoved(result):
20  if len(result.result.end_joint.position) == 2:
21  __builtin__.ptuPos = (result.result.end_joint.position[0], result.result.end_joint.position[1])
22  else:
23  print("invalid ptu result")
24 rospy.Subscriber("/ptu_controller_actionlib/result", PTUMovementActionResult, ptuMoved)
25 
26 # to get base position
27 __builtin__.basePos = (0, 0, 0)
28 def baseMoved(idc):
29  thread = threading.Thread(target=getBasePos)
30  thread.start()
31  thread.join()
32 def getBasePos():
33  tfListener = tf.TransformListener()
34  t = rospy.Time()
35  tfListener.waitForTransform("/map", "/base_link", rospy.Time(), rospy.Duration(4.0))
36  pos, quat = tfListener.lookupTransform("/map", "/base_link", t)
37  orientationAngle = math.degrees(math.asin(quat[2]) * 2)
38  __builtin__.basePos = (pos[0], pos[1], orientationAngle)
39 rospy.Subscriber("/move_base/result", MoveBaseActionResult, baseMoved)
40 rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, baseMoved)
41 
42 # to set base
43 set_base_client = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=1)
44 
45 # ptu
46 def movePTU(pan, tilt):
47  newPTUPos = (__builtin__.ptuPos[0] + pan, __builtin__.ptuPos[1] + tilt)
48  setPTU(*newPTUPos)
49 
50 def setPTU(pan, tilt):
51  ptu_client = actionlib.SimpleActionClient("ptu_controller_actionlib", PTUMovementAction)
52  if not ptu_client.wait_for_server():
53  print("Could not connect to ptu action server")
54  ptu_goal = PTUMovementGoal()
55  ptu_goal.target_joint.header.seq = 0
56  ptu_goal.target_joint.name = ["pan", "tilt"]
57  ptu_goal.target_joint.velocity = [0, 0]
58  ptu_goal.target_joint.position = [float(pan), float(tilt)]
59  result = ptu_client.send_goal_and_wait(ptu_goal)
60  if result != 3:
61  print("setPTU failed")
62  # visualization
63  #waitForService("/asr_robot_model_services/GetCameraPose")
64  waitForService("/nbv/trigger_frustum_visualization", True)
65  subprocess.call("python " + resources_for_active_scene_recognition_path + "/src/getFrustum.py", shell=True)
66  return ptu_client.get_result()
67 
68 # base
69 def moveBase(x, y, rotation):
70  move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
71  if not move_base_client.wait_for_server():
72  print("Could not connect to move base action server")
73  move_base_goal = MoveBaseGoal()
74  move_base_goal.target_pose.header.frame_id = "/map"
75  move_base_goal.target_pose.pose.position.x = x
76  move_base_goal.target_pose.pose.position.y = y
77  move_base_goal.target_pose.pose.orientation.w = math.cos(math.radians(rotation) / 2.0)
78  move_base_goal.target_pose.pose.orientation.z = math.sin(math.radians(rotation) / 2.0)
79  result = move_base_client.send_goal_and_wait(move_base_goal)
80  if result != 3:
81  print("moveBase failed")
82  return move_base_client.get_result()
83 
84 def setBase(x, y, rotation):
85  set_base_goal = PoseWithCovarianceStamped()
86  set_base_goal.header.frame_id = "map"
87  set_base_goal.pose.pose.position.x = float(x)
88  set_base_goal.pose.pose.position.y = float(y)
89  set_base_goal.pose.pose.orientation.w = math.cos(math.radians(float(rotation)) / 2.0)
90  set_base_goal.pose.pose.orientation.z = math.sin(math.radians(float(rotation)) / 2.0)
91  set_base_client.publish(set_base_goal)
92 
93 def setPositionAndOrientation(positionAndOrientation):
94  setBase(*positionAndOrientation[0])
95  setPTU(*positionAndOrientation[1])
def moveBase(x, y, rotation)
Definition: move.py:69
def waitForService(serviceName, silence=False)
Definition: general_ros.py:5
def setPositionAndOrientation(positionAndOrientation)
Definition: move.py:93
def setBase(x, y, rotation)
Definition: move.py:84
def getBasePos()
Definition: move.py:32
def baseMoved(idc)
Definition: move.py:28
def setPTU(pan, tilt)
Definition: move.py:50
def movePTU(pan, tilt)
Definition: move.py:46
def ptuMoved(result)
Definition: move.py:19


asr_resources_for_active_scene_recognition
Author(s): Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Karrenbauer Oliver, Marek Felix, Mei├čner Pascal, Stroh Daniel, Trautmann Jeremias
autogenerated on Tue Feb 18 2020 03:14:15