-
Notifications
You must be signed in to change notification settings - Fork 4
/
offset_poses
34 lines (27 loc) · 1020 Bytes
/
offset_poses
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#!/usr/bin/env python
"""
Add the start position offset to MAVROS position
and to the trajectory tracker command. Useful
mainly for visualization.
"""
import rospy
from geometry_msgs.msg import PoseStamped
pub_ref = rospy.Publisher('/autopilot/reference', PoseStamped, queue_size=10)
pub_pose = rospy.Publisher('/autopilot/pose', PoseStamped, queue_size=10)
startx = float(rospy.get_param('/start/x', 0))
starty = float(rospy.get_param('/start/y', 0))
startz = float(rospy.get_param('/start/init_z', 0))
def ref_cb(data):
data.pose.position.x += startx
data.pose.position.y += starty
data.pose.position.z += startz
pub_ref.publish(data)
def pose_cb(data):
data.pose.position.x += startx
data.pose.position.y += starty
data.pose.position.z += startz
pub_pose.publish(data)
rospy.init_node('offset_poses', anonymous=True)
sub_ref = rospy.Subscriber('/reference/pose', PoseStamped, ref_cb)
sub_pose = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_cb)
rospy.spin()