def _odom_sub(self, msg):
self.position = msg.pose.pose.position
quaternion = msg.pose.pose.orientation
orientatioN = efq(
[quaternion.x, quaternion.y, quaternion.z, quaternion.w])
self.rotation = orientatioN[2]
def callback_odom(self, msg):
self.xr = msg.pose.pose.position.x
self.yr = msg.pose.pose.position.y
quat = msg.pose.pose.orientation
self.phi = efq([quat.x, quat.y, quat.z, quat.w])[2]
self.change_odom = True
return
def get_current_goal(self):
self.current_goal = self.goals[self.goal_id]
self.x_goal, self.y_goal = self.current_goal[0], self.current_goal[1]
self.qz_goal, self.qw_goal = self.current_goal[2], self.current_goal[3]
self.theta_goal = efq([0, 0, self.qz_goal, self.qw_goal])
self.theta_goal = self.theta_goal[2]
return
def __odom_sub(self, msg):
self.position = msg.pose.pose.position
quaternion = msg.pose.pose.orientation
orientatioN = efq(
[quaternion.x, quaternion.y, quaternion.z, quaternion.w])
self.orientation = Orientation(orientatioN[0], orientatioN[1],
orientatioN[2])
def simulate_vehicle(self, current_pose, cmd_vel_list, period=0.05):
"""
Takes the current position, a list of user inputs, and a step size
and simulates the vehicle moving forward in time.
"""
now = rospy.Time.now().to_sec()
time = current_pose.header.stamp.to_sec()
current_cmd_vel = cmd_vel_list.pop(0)
sim_path = []
state = [current_pose.transform.translation.x,
current_pose.transform.translation.y,
efq([current_pose.transform.rotation.x,
current_pose.transform.rotation.y,
current_pose.transform.rotation.z,
current_pose.transform.rotation.w])]
dk = DifferentialKinematics(1.0, 1.0, state[0], state[1], state[2][2])
# print('Entering sim...')
while time < now and not rospy.is_shutdown():
# print(time, now, now - time)
# Increase time
time += period
# Check for new input
if cmd_vel_list and current_cmd_vel[0] < time:
while cmd_vel_list and cmd_vel_list[0][0] <= time:
current_cmd_vel = cmd_vel_list.pop(0)
# Use the input, the state, and the period to step the model
dk.linear_velocity = current_cmd_vel[1].linear.x
dk.angular_velocity = current_cmd_vel[1].angular.z
x, y, w = dk.step_simulation(period)
sim_path.append([time, x, y, w])
# print('Exiting sim...')
return sim_path
def callbackOdom(self, msg):
self.x_bot = msg.pose.pose.position.x
self.y_bot = msg.pose.pose.position.y
quat = msg.pose.pose.orientation
self.theta_bot = efq([quat.x, quat.y, quat.z, quat.w])[2]
self.change_odom = True
return
def get_imu_orientation(self):
if self.imu_orientation is None:
return 0, 0, 0
else:
quat = self.imu_orientation.orientation
roll, pitch, yaw = efq((quat.x, quat.y, quat.z, quat.w))
return roll, pitch, yaw
def get_robot_orientation(self):
quat = self.current_pose.pose.pose.orientation
roll, pitch, yaw = efq((quat.x, quat.y, quat.z, quat.w))
x1, y1 = [np.cos(yaw), np.sin(yaw)]
# Give results in quaterion, euler and vector form
return quat, (roll, pitch, yaw), [x1, y1]
def callback_pose_user(self, msg):
self.pose_user = msg
theta_aux = efq(
[0, 0, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
#self.thetaUser = np.arccos(theta_aux)*2
self.thetaUser = theta_aux[2]
self.change1 = True
return
def callback_odom(self, msg):
self.pose_walker = msg
theta_aux = efq(
[0, 0, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
#self.thetaWalker = np.arccos(theta_aux)*2
self.thetaWalker = theta_aux[2]
self.change2 = True
return
def callback_odom(self, msg):
self.x_bot = msg.pose.pose.position.x
self.y_bot = msg.pose.pose.position.y
self.qz_bot = msg.pose.pose.orientation.z
self.qw_bot = msg.pose.pose.orientation.w
self.theta_bot = efq([0, 0, self.qz_bot, self.qw_bot])
self.theta_bot = self.theta_bot[2]
self.change = True
return
def __odom_sub(self, msg):
# Calbacck function for odomentry
self.position = msg.pose.pose.position
quaternion = msg.pose.pose.orientation
orientatioN = efq(
[quaternion.x, quaternion.y, quaternion.z, quaternion.w])
self.orientation = Orientation(orientatioN[0], orientatioN[1],
orientatioN[2])
def callback_tf(self, msg):
if msg.transforms[0].header.frame_id == "map" and msg.transforms[
0].child_frame_id == "odom":
qz = msg.transforms[0].transform.rotation.z
qw = msg.transforms[0].transform.rotation.w
self.rotMapOdom = efq([0, 0, qz, qw])[2]
self.tx = msg.transforms[0].transform.translation.x
self.ty = msg.transforms[0].transform.translation.y
self.change5 = True
return
def callback_path(self, msg):
self.path = msg
try:
n = len(self.path.poses)
self.nextPose = self.path.poses[n - 1].pose
except:
self.nextPose = self.path.poses[0].pose
theta_aux = efq(
[0, 0, self.nextPose.orientation.z, self.nextPose.orientation.w])
self.thetaNav = theta_aux[2]
self.change3 = True
return
def imu_cb(self, msg):
try:
(trans, rot) = self.listener.lookupTransform(msg.header.frame_id, '/base_footprint', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr('Exception in TF lookup')
q = (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w)
(r, p, y) = efq(q)
# Raw Reading from the IMU:
# 0 is north, and rotations in the CCW direction are positive.
# I guess that means N-W-U
rospy.loginfo("Raw: " + str(wrapTo360(180.0 * y / math.pi)))
# N-W-U to E-N-U is simply a rotation of -90 about the Z axis.
tform = quaternion_about_axis(math.pi/2, (0,0,1))
enu = quaternion_multiply(q,tform)
(r,p,y) = efq(enu)
rospy.loginfo("ENU: " + str(radAndWrap(y)))
y = (-y) + math.pi/2.0
rospy.loginfo("NED: " + str(radAndWrap(y)))
def __call__(self, err):
"""
Assume this is getting a tf2_ros transform
"""
t = err.header.stamp.to_sec()
dxyz = err.transform.translation
o = err.transform.rotation
_, _, T = efq([o.x, o.y, o.z, o.w])
self.dx(dxyz.x, t)
self.dy(dxyz.y, t)
self.dz(dxyz.z, t)
self.dT(T, t)
return self.calc_control(self.dx, self.dy, self.dz, self.dT)
def __call__(self,err):
"""
Assume this is getting a tf2_ros transform
"""
t = err.header.stamp.to_sec()
dxyz = err.transform.translation
o = err.transform.rotation
_,_,T = efq([o.x,o.y,o.z,o.w])
self.dx(dxyz.x,t)
self.dy(dxyz.y,t)
self.dz(dxyz.z,t)
self.dT(T,t)
return self.calc_control(self.dx,self.dy,self.dz,self.dT)
def callback(msg):
global last_time, yaw, pub, last_yaw, count, pub_pose
now = msg.header.stamp
if last_time == None:
last_time = now
yaw = 0
last_yaw = 0
count = 0
return
count += 1
q = msg.orientation
delta_yaw = efq((q.x, q.y, q.z, q.w))[2] * (now - last_time).to_sec()
# delta_yaw = efq((q.x, q.y, q.z, q.w))[2] * 1.0/200.0
yaw += delta_yaw
last_time = now
if count != 10:
return
count = 0
delta_yaw = last_yaw - yaw
last_yaw = yaw
covar = 1.0/abs(radians(delta_yaw))
print delta_yaw, radians(delta_yaw), covar
q = qfe(0, 0, radians(yaw))
header = msg.header
msg = PoseStamped()
msg.header = header
msg.pose.orientation.x = q[0]
msg.pose.orientation.y = q[1]
msg.pose.orientation.z = q[2]
msg.pose.orientation.w = q[3]
pub_pose.publish(msg)
msg = Imu()
msg.header = header
msg.orientation.x = q[0]
msg.orientation.y = q[1]
msg.orientation.z = q[2]
msg.orientation.w = q[3]
msg.orientation_covariance = [1e+100, 0, 0,
0, 1e+100, 0,
0, 0, covar]
pub.publish(msg)
def pose_callback(self,pose):
if self.goal1 is None and self.goal2 is None:
return
# Convert pose to x,y,theta (from quaternion orientation)
quaternion = N.asarray((pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w))
pose = N.asarray([pose.pose.pose.position.x, pose.pose.pose.position.y,efq(quaternion)[2]])
self.goals.update_pose(pose)
if self.goals.next_goal_flag:
self.string_msg.data = str(self.goals.last_distance)
self.acc_pub.publish(self.string_msg)
self.string_msg.data = str(self.goals.current_goal)
self.goal_pub.publish(self.string_msg)
def imu_cb(self, data):
if self.decimate_ahrs not in [0, 1]:
if self.ahrs_count % self.decimate_ahrs == 0:
self.ahrs_count = 1
else:
self.ahrs_count += 1
return
if not self.imu_used:
if self.heading_initilized:
return
if not self.heading_initilized:
self.heading_initilized = True
(r, p, yaw) = efq([data.orientation.x, data.orientation.y, \
data.orientation.z, data.orientation.w])
# This is supposed to correct for the coordinate differences
self.ekf.measurementUpdateAHRS(yaw + np.pi / 2)
self.filter_time = data.header.stamp
return
def listen(self, msg):
# quaternion = [0, 0, 0, 0]
# quaternion[0] = msg.pose.orientation.x
# quaternion[1] = msg.pose.orientation.y
# quaternion[2] = msg.pose.orientation.z
# quaternion[3] = msg.pose.orientation.w
euler = efq((msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w))
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
x = msg.pose.position.x
y = msg.pose.position.y
z = msg.pose.position.z
#self.setpoint_pos_pub.publish(0)
#self.state_pos_pub.publish(z)
self.setpoint_angle_pub.publish(0)
self.state_angle_pub.publish(x - (0.531 * z + 0.02043))
def transform(PoseStamped): pose = PoseStamped.pose pos = pose.position orient = pose.orientation # Simple rotations from ENU to NED... ish tfpos = array([pos.y,pos.x,-pos.z]) tforient = efq([orient.x,orient.y,orient.z,orient.w]) # Transform from quat to euler happens in ENU frame. # Rotate to match NED tforient = array([-tforient[2],tforient[0],tforient[1]]) # Desired position. This should be read in from a file, or better # yet. should come from a subscription to a topic somehow. Still learning... des_pos = array([0,0,-1.5]) pos_err = des_pos - tfpos # Keep yaw turned towards +x des_yaw = 0 yaw_err = des_yaw - tforient[0] vel_set(pos_err,yaw_err)
def pose_callback(self, pose):
if self.goal1 is None and self.goal2 is None:
return
# Convert pose to x,y,theta (from quaternion orientation)
quaternion = N.asarray(
(pose.pose.pose.orientation.x, pose.pose.pose.orientation.y,
pose.pose.pose.orientation.z, pose.pose.pose.orientation.w))
pose = N.asarray([
pose.pose.pose.position.x, pose.pose.pose.position.y,
efq(quaternion)[2]
])
self.goals.update_pose(pose)
if self.goals.next_goal_flag:
self.string_msg.data = str(self.goals.last_distance)
self.acc_pub.publish(self.string_msg)
self.string_msg.data = str(self.goals.current_goal)
self.goal_pub.publish(self.string_msg)
def detect_target(self,data):
# find yellow blob
hsv = cv2.cvtColor(data,cv2.COLOR_BGR2HSV)
yel_lo = (20,100,100)
yel_hi = (30,255,255)
mask = cv2.inRange(hsv,yel_lo,yel_hi)
_,cnts,heir = cv2.findContours(mask,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
if not len(cnts): # no yellow(target) in image
return data,None,None,None
target = sorted(cnts,key=lambda c:cv2.contourArea(c))[-1]
leftmost = tuple(target[target[:,:,0].argmin()][0])
rightmost = tuple(target[target[:,:,0].argmax()][0])
topmost = tuple(target[target[:,:,1].argmin()][0])
bottommost = tuple(target[target[:,:,1].argmax()][0])
if (leftmost[0] == 0 or
topmost[1] == 0 or
rightmost[0] == self.cam_info.width or
bottommost[1] == self.cam_info.height): #target is at least partially occluded by edge of frame
# rospy.loginfo('{0}: frame occlusion. not trusting estimate'.format('vision_estimator'))
return data,None,None,None
try:
ellipse_rect = cv2.fitEllipse(target)
except: #FIXME: need to capture only OpenCV Error
#error: OpenCV Error: Incorrect size of input array (There should be at least 5 points to fit the ellipse) in fitEllipse
return data,None,None,None
#TODO: handle case where we are too close to see the platform and also when we just can't see a platform at all
if not abs(1 - (ellipse_rect[1][0] - ellipse_rect[1][1])) > 0.2:
dist = self.dz - 0.1 #assume we're really close and we've lost the platform a little
else:
area = cv2.contourArea(target)
diam = np.sqrt(4*area/np.pi)
dist = 0.255*2*self.fy/diam # actual radius of landing pad = 255mm
dz = dist # not a very good guess, especially if we're tilted
M = cv2.moments(target)
if not M["m00"]:
return mask,None, None,None
Pi = np.matrix((M["m10"]/M["m00"],M["m01"]/M["m00"],1)).T # point in image coords
Pc = self.K.I*Pi # point in world units (still on sensor)
Pc = Pc/Pc[2] * dist # point projected onto ground plane
Pc[1] *= -1 # account for image coordinates astarting in top left
o = self.orientation
q = [o.x,o.y,o.z,o.w]
r,p,y = efq(q,'sxyz')
Rcam_body = np.matrix(eul_mat(3.14,0,0))[:3,:3]
Rbody_inert = np.matrix(eul_mat(r,p,-y))[:3,:3]
Pr = Rbody_inert*Rcam_body.T*Pc
dx = Pr[0]
dy = -Pr[1] # invert y to make sane coordinate system (right:x+,up:y+)
#print('dx',float(Pr[0]),' dy',float(Pr[1]),'rpy',r,p,-y)
# build message for estimator
pose = Pose()
# rotate +x to +y and +y to -x to reflect the fact that +x is out the nose of the vehicle
pose.position.x = -dy
pose.position.y = dx
pose.position.z = dz
pose.orientation = o
pose_w_cov = PoseWithCovariance(
pose=pose,
covariance=self.cov)
odom = Odometry(pose=pose_w_cov)
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = 'pad'
odom.child_frame_id = 'camera::camera_link'
self.odom_pub.publish(odom)
ci = ((data.shape[0]/2),(data.shape[1]/2))
heading = (Pi[0]-ci[0],ci[1]-Pi[1])
return mask,heading,tuple(Pi[:2]),ci
def wp_pub_sub():
global curr_x,curr_y,curr_orient,next_wp,ready_pub,spin,mode,size,rtl,all_waypoints,first_pub
next_wp.pose.position.z = 0.4
all_waypoints = Waypoints()
rospy.init_node('UAV_setpoint',anonymous=True)
rospy.Subscriber('next_wps',Waypoints,setpoints)
rospy.Subscriber('/mavros/state',State,get_curr_mode)
rospy.Subscriber('slam_out_pose',PoseStamped,set_curr)
rospy.Subscriber('/mavros/local_position/pose',PoseStamped,set_curr_z)
rospy.Subscriber('return_to_launch',Bool,get_rtl)
rate = rospy.Rate(15)
wp_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
ready_pub = rospy.Publisher('ready_for_wps', Bool, queue_size=10)
rtl_pub = rospy.Publisher('return_to_launch', Bool, queue_size=10)
rtl_path_pub = rospy.Publisher('next_wps', Waypoints, queue_size=10)
first_pub = rospy.Publisher('first_wps', Bool, queue_size=10)
spin_flag = 0
spin = False
spins = 0
offboard_counter = 0
rtl_flag = 0
size = 128
while not rospy.is_shutdown():
if 'mode' in globals() and mode in "OFFBOARD":
offboard_counter += 1
#rospy.loginfo(str(offboard_counter))
if 0 > offboard_counter <= 5:
rospy.loginfo("Just entered OFFBOARD mode")
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = next_wp.pose.position.x #these lines could also be changed instead of above
pos.pose.position.y = next_wp.pose.position.y
pos.pose.position.z = next_wp.pose.position.z #this line can be changed to reflect desired z setpoints
#if 'curr_z' in globals():
# rospy.loginfo('spins:%s,curr_z:%s,offboard_counter:%s',str(spins),str(curr_z),str(offboard_counter))
if spins == 0 and 'curr_z' in globals() and curr_z > 0.15 and offboard_counter >= 50:
rospy.loginfo("executing first spin maneuver")
spin = True
first_pub.publish(True)
if spins%5:
spin = False
if spin:
# Yaw is in /slam_out_pose frame.
if spin_flag == 1:
rospy.loginfo("Yaw Left")
yaw = pi/8
elif spin_flag == 0:
rospy.loginfo("Yaw Right")
yaw = -pi/8
else:
rospy.loginfo("Yaw Back")
yaw = 0
quat = qfe(0,0,yaw+pi/2)#euler angles -- RPY roll pitch yaw --> Q xyzw
#add pi/2 to send to setpoint_position/local
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
wp_pub.publish(pos)
while not 'curr_orient' in globals():
time.sleep(0.01)
curr_yaw = efq([curr_orient.x,curr_orient.y,curr_orient.z,curr_orient.w])[-1]
#rospy.loginfo('curr_yaw: %s, des_yaw: %s',str(curr_yaw),str(yaw))
epsilon = 0.1
if abs(curr_yaw - yaw) < epsilon:
spin_flag += 1
if yaw == 0:
spin = False
spin_flag = 0
ready_pub.publish(True) #ready for more waypoints. send nav_map to sentel
spins += 1
rate.sleep()
continue # this tells us to skip everything else in the loop and start from the top, (skip next 7 lines)
if 'rtl' in globals() and rtl and not rtl_flag:
rtl_flag = 1
rtl_pub.publish(True)
rtl_wps = Waypoints()
rtl_wps.x = list(reversed(all_waypoints.x))
rtl_wps.y = list(reversed(all_waypoints.y))
rtl_path_pub.publish(rtl_wps)
quat = qfe(0,0,pi/2)
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
wp_pub.publish(pos)
rate.sleep()
def wp_pub_sub():
global curr_x,curr_y,curr_orient,next_wp,ready_pub,spin,state_id
next_wp.pose.position.z = 0.5
rospy.init_node('UAV_setpoint',log_level=rospy.DEBUG,anonymous=True)
rospy.Subscriber('next_wps',Waypoints,setpoints)
rospy.Subscriber('slam_out_pose',PoseStamped,set_curr)
rate = rospy.Rate(15)
wp_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
ready_pub = rospy.Publisher('ready_for_wps', Bool, queue_size=10)
spin_flag = 0
spin = False
while not rospy.is_shutdown():
if state_id is 1:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=0
quat = qfe(0,0,pi/2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
elif state_id is 2:
count = +1
if count < 150:
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x=0
pos.pose.position.y=0
pos.pose.position.z=0
quat = qfe(0,0,pi/2)
pos.pose.orientation.w = quat[3]
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pub.publish(pos)
pos = PoseStamped()
pos.header.stamp = rospy.Time.now()
pos.pose.position.x = -next_wp.pose.position.y #these lines could also be changed instead of above
pos.pose.position.y = next_wp.pose.position.x
pos.pose.position.z = next_wp.pose.position.z #this line can be changed to reflect desired z setpoints
if curr_z > 0.3:
spin = True
if spin:
# Yaw is in /slam_out_pose frame.
if spin_flag == 1:
rospy.loginfo("Yaw Left")
yaw = 3*pi/4
elif spin_flag == 0:
rospy.loginfo("Yaw Right")
yaw = -3*pi/4
else:
rospy.loginfo("Yaw Back")
yaw = 0
quat = qfe(0,0,yaw+pi/2)#euler angles -- RPY roll pitch yaw --> Q xyzw
#add pi/2 to send to setpoint_position/local
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
wp_pub.publish(pos)
while not 'curr_orient' in globals():
time.sleep(0.01)
curr_yaw = efq([curr_orient.x,curr_orient.y,curr_orient.z,curr_orient.w])[-1]
rospy.loginfo('curr: %s, des: %s',str(curr_yaw),str(yaw))
epsilon = 0.1
if abs(curr_yaw - yaw) < epsilon:
spin_flag += 1
if yaw == 0:
spin = False
spin_flag = 0
ready_pub.publish(True) #ready for more waypoints. send nav_map to sentel
rate.sleep()
continue # this tells us to skip everything else in the loop and start from the top, (skip next 7 lines)
quat = qfe(0,0,pi/2)
pos.pose.orientation.x = quat[0]
pos.pose.orientation.y = quat[1]
pos.pose.orientation.z = quat[2]
pos.pose.orientation.w = quat[3]
wp_pub.publish(pos)
rate.sleep()
import rospy
import tf2_ros
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion as efq
rospy.init_node('tf_dist')
tf_dist_pub = rospy.Publisher('irols/tf_dist', Twist, queue_size=1)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
while not rospy.is_shutdown():
rate = rospy.Rate(10)
dist = Twist()
try:
t = tfBuffer.lookup_transform('pad', 'base_link', rospy.Time(0))
dist.linear.x = t.transform.translation.x
dist.linear.y = t.transform.translation.y
dist.linear.z = t.transform.translation.z
o = t.transform.rotation
dist.angular.x, dist.angular.y, dist.angular.z = efq(
[o.x, o.y, o.z, o.w])
tf_dist_pub.publish(dist)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
print e
rate.sleep()
continue
def __odom_callback(self, msg):
self.position = msg.pose.pose.position
orientatioN = efq([msg.pose.pose.orientaion.x, msg.pose.pose.orientaion.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation])
self.orientation = Orientation(orientatioN[0], orientatioN[1], orientatioN[2])
def callbackGoal(self, msg): self.x_goal = msg.pose.position.x self.y_goal = msg.pose.position.y quat = msg.pose.orientation self.theta_goal = efq([quat.x, quat.y, quat.z, quat.w])[2] self.change_goal = True
#!/usr/bin/env python
import rospy
import tf2_ros
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion as efq
rospy.init_node('tf_dist')
tf_dist_pub = rospy.Publisher('irols/tf_dist',Twist,queue_size=1)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
while not rospy.is_shutdown():
rate = rospy.Rate(10)
dist = Twist()
try:
t = tfBuffer.lookup_transform('pad','base_link',rospy.Time(0))
dist.linear.x = t.transform.translation.x
dist.linear.y = t.transform.translation.y
dist.linear.z = t.transform.translation.z
o = t.transform.rotation
dist.angular.x, dist.angular.y, dist.angular.z = efq([o.x,o.y,o.z,o.w])
tf_dist_pub.publish(dist)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
print e
rate.sleep()
continue
