def transform_quaternion_stamped(self, new_frame, quat_stamped, robot_state):
"""
Transforms a quaternion into new_frame_id according to robot state.
**Args:**
**new_frame_id (string):** new frame id
**quat_stamped (geometry_msgs.msg.QuaternionStamped):** quaternion stamped (current frame is defined by
header)
**robot_state (arm_navigation_msgs.msg.RobotState):** Robot state used for transformations
**Returns:**
A geometry_msgs.msg.QuaternionStamped defined in new_frame_id
**Raises:**
**exceptions.TransformStateError:** if there is an error getting the transform
"""
qs = QuaternionStamped()
qs.header.frame_id = new_frame
qs.quaternion = self.transform_quaternion(
new_frame, quat_stamped.header.frame_id, quat_stamped.quaternion, robot_state
)
return qs
def do_transform_imu(imu_msg, transform):
"""
Transforms the given Imu message into the given target frame
:param imu_msg:
:type imu_msg: Imu
:param transform:
:type transform: TransformStamped
:return: Imu message transformed into the target frame
:rtype: Imu
"""
src_l = Vector3Stamped()
src_l.header = imu_msg.header
src_l.vector = imu_msg.linear_acceleration
src_a = Vector3Stamped()
src_a.header = imu_msg.header
src_a.vector = imu_msg.angular_velocity
src_o = QuaternionStamped()
src_o.header = imu_msg.header
src_o.quaternion = imu_msg.orientation
tgt_l = do_transform_vector3(src_l, transform)
tgt_a = do_transform_vector3(src_a, transform)
tgt_o = do_transform_quaternion(src_o, transform)
tgt_imu = copy.deepcopy(imu_msg)
tgt_imu.header.frame_id = transform.child_frame_id
tgt_imu.linear_acceleration = tgt_l
tgt_imu.angular_velocity = tgt_a
tgt_imu.orientation = tgt_o
return tgt_imu
def align_object(object_world, listener):
'''
Align the object frame coordinates to be consistent relative to the world frame.
'''
object_world_aligned = copy.deepcopy(object_world)
#Transform object frame from camera frame to world frame
#listener = tf.TransformListener()
obj_qnt_stamped_world = QuaternionStamped()
obj_qnt_stamped_world.header.frame_id = object_world.header.frame_id
obj_qnt_stamped_world.quaternion = copy.deepcopy(
object_world.pose.orientation)
#object_qnt_world = listener.transformQuaternion('world', obj_qnt_stamped_cam)
#Convert from quarternion to matrix
quarternion = [
obj_qnt_stamped_world.quaternion.x, obj_qnt_stamped_world.quaternion.y,
obj_qnt_stamped_world.quaternion.z, obj_qnt_stamped_world.quaternion.w
]
trans_mat = tf.transformations.quaternion_matrix(quarternion)
rot_mat = trans_mat[:3, :3]
#Convert from matrix to quarternion
# Adapt to the new robot arm pose:
# robot arm 1st joint zero angles faces the table.
align_trans_matrix = np.identity(4)
object_size = [object_world.width, object_world.height, object_world.depth]
#Find and align x axes.
x_axis = [1., 0., 0.]
align_x_axis, min_ang_axis_idx = find_min_ang_vec(x_axis, rot_mat)
rot_mat = np.delete(rot_mat, min_ang_axis_idx, axis=1)
#align_trans_matrix[:3, 1] = align_x_axis
align_trans_matrix[:3, 0] = align_x_axis
object_world_aligned.height = object_size[min_ang_axis_idx]
#y axes
y_axis = [0., 1., 0.]
align_y_axis, min_ang_axis_idx = find_min_ang_vec(y_axis, rot_mat)
rot_mat = np.delete(rot_mat, min_ang_axis_idx, axis=1)
#align_trans_matrix[:3, 0] = -align_y_axis
align_trans_matrix[:3, 1] = align_y_axis
object_world_aligned.width = object_size[min_ang_axis_idx]
#z axes
z_axis = [0., 0., 1.]
align_z_axis, min_ang_axis_idx = find_min_ang_vec(z_axis, rot_mat)
align_trans_matrix[:3, 2] = align_z_axis
object_world_aligned.depth = object_size[min_ang_axis_idx]
align_qtn_array = tf.transformations.quaternion_from_matrix(
align_trans_matrix)
align_qnt_stamped = QuaternionStamped()
align_qnt_stamped.header.frame_id = object_world.header.frame_id
align_qnt_stamped.quaternion.x, align_qnt_stamped.quaternion.y, \
align_qnt_stamped.quaternion.z, align_qnt_stamped.quaternion.w = align_qtn_array
object_world_aligned.pose.orientation = align_qnt_stamped.quaternion
return object_world_aligned
def transform_quaternion_stamped(self, new_frame, quat_stamped,
robot_state):
'''
Transforms a quaternion into new_frame_id according to robot state.
**Args:**
**new_frame_id (string):** new frame id
**quat_stamped (geometry_msgs.msg.QuaternionStamped):** quaternion stamped (current frame is defined by
header)
**robot_state (arm_navigation_msgs.msg.RobotState):** Robot state used for transformations
**Returns:**
A geometry_msgs.msg.QuaternionStamped defined in new_frame_id
**Raises:**
**exceptions.TransformStateError:** if there is an error getting the transform
'''
qs = QuaternionStamped()
qs.header.frame_id = new_frame
qs.quaternion = self.transform_quaternion(new_frame,
quat_stamped.header.frame_id,
quat_stamped.quaternion,
robot_state)
return qs
def publish(self, imu_msg, nav_msg):
if self.tfBuffer.can_transform(nav_msg.header.frame_id, "odom",
rospy.Time.now(),
rospy.Duration.from_sec(0.5)):
if self.tfBuffer.can_transform(imu_msg.header.frame_id,
"base_link", rospy.Time.now(),
rospy.Duration.from_sec(0.5)):
unfiltered_msg = Odometry()
unfiltered_msg.header.frame_id = "odom"
unfiltered_msg.child_frame_id = "base_link"
imu_q = QuaternionStamped()
imu_q.quaternion = imu_msg.orientation
imu_q.header = imu_msg.header
unfiltered_msg.pose.pose.orientation = self.tfListener.transformQuaternion(
"base_link",
imu_q).quaternion #TF2 FOR KINETIC JUST AIN'T WORKING
nav_p = PointStamped()
nav_p.point = nav_msg.pose.pose.position
nav_p.header = nav_msg.header
unfiltered_msg.pose.pose.position = self.tfListener.transformPoint(
"odom", nav_p).point
self.pub.publish(unfiltered_msg)
else:
rospy.logwarn("{} to base_link tf unavailable!".format(
imu_msg.header.frame_id))
else:
rospy.logwarn("{} to odom tf unavailable!".format(
nav_msg.header.frame_id))
def imu_callback(self, msg):
raw_imu_quat = QuaternionStamped()
raw_imu_quat.header = msg.header
raw_imu_quat.quaternion = msg.orientation
raw_imu_av = Vector3Stamped()
raw_imu_av.header = msg.header
raw_imu_av.vector = msg.angular_velocity
raw_imu_acc = Vector3Stamped()
raw_imu_acc.header = msg.header
raw_imu_acc.vector = msg.linear_acceleration
try:
self.listener.waitForTransform(self.base_link_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(1.0)) # gyrometer->body
imu_rootlink_quat = self.listener.transformQuaternion(self.base_link_frame, raw_imu_quat)
imu_rootlink_av = self.listener.transformVector3(self.base_link_frame, raw_imu_av)
imu_rootlink_acc = self.listener.transformVector3(self.base_link_frame, raw_imu_acc)
# todo: convert covariance
except:
rospy.logwarn("[%s] failed to solve imu_to_base tf: %s to %s", rospy.get_name(), msg.header.frame_id, self.base_link_frame)
return
msg.header = imu_rootlink_quat.header
msg.orientation = imu_rootlink_quat.quaternion
msg.angular_velocity = imu_rootlink_av.vector
msg.linear_acceleration = imu_rootlink_acc.vector
# publish
self.pub.publish(msg)
def make_quaternion_msg(q, frame_id, time=0.0):
""" Create a QuaternionStamped message. """
msg = QuaternionStamped()
msg.header.stamp = rospy.Time.from_sec(time)
msg.header.frame_id = frame_id
msg.quaternion = Quaternion(q[1], q[2], q[3], q[0])
return msg
def on_enter(self, userdata):
pose = userdata.target_pose # type: PoseStamped
position = PointStamped()
position.header.frame_id = pose.header.frame_id
position.point = pose.pose.position
orientation = QuaternionStamped()
orientation.header.frame_id = pose.header.frame_id
orientation.quaternion = pose.pose.orientation
userdata.target_position = position
userdata.target_orientation = orientation
def do_transform_quaternion(quaternion_msg, transform):
# Use PoseStamped to transform the Quaternion
src_ps = PoseStamped()
src_ps.header = quaternion_msg.header
src_ps.pose.orientation = quaternion_msg.quaternion
tgt_ps = do_transform_pose(src_ps, transform)
tgt_o = QuaternionStamped()
tgt_o.header = tgt_ps.header
tgt_o.quaternion = tgt_ps.pose.orientation
return tgt_o
def __init__(self):
self.current_heading = QuaternionStamped()
self.best_heading = QuaternionStamped()
self.vel_sub = rospy.Subscriber('tcpvel', TwistStamped, self.vel_callback)
self.head_pub = rospy.Publisher('heading', QuaternionStamped, queue_size=1)
self.VelYaw = 0.0
self.heading_last_good = QuaternionStamped()
def align_obj_ort(object_world_pose, listener):
'''
Align the object frame coordinates to be consistent relative to the world frame.
'''
obj_qnt_stamped_world = QuaternionStamped()
obj_qnt_stamped_world.header.frame_id = object_world_pose.header.frame_id
obj_qnt_stamped_world.quaternion = copy.deepcopy(
object_world_pose.pose.orientation)
#Convert from quarternion to matrix
quarternion = [
obj_qnt_stamped_world.quaternion.x, obj_qnt_stamped_world.quaternion.y,
obj_qnt_stamped_world.quaternion.z, obj_qnt_stamped_world.quaternion.w
]
trans_mat = tf.transformations.quaternion_matrix(quarternion)
rot_mat = trans_mat[:3, :3]
#Convert from matrix to quarternion
# Adapt to the new robot arm pose:
# robot arm 1st joint zero angles faces the table.
align_trans_matrix = np.identity(4)
#Find and align x axes.
x_axis = [1., 0., 0.]
align_x_axis, min_ang_axis_idx = find_min_ang_vec(x_axis, rot_mat)
rot_mat = np.delete(rot_mat, min_ang_axis_idx, axis=1)
align_trans_matrix[:3, 1] = align_x_axis
#y axes
y_axis = [0., 1., 0.]
align_y_axis, min_ang_axis_idx = find_min_ang_vec(y_axis, rot_mat)
rot_mat = np.delete(rot_mat, min_ang_axis_idx, axis=1)
align_trans_matrix[:3, 0] = -align_y_axis
#z axes
z_axis = [0., 0., 1.]
align_z_axis, min_ang_axis_idx = find_min_ang_vec(z_axis, rot_mat)
align_trans_matrix[:3, 2] = align_z_axis
align_qtn_array = tf.transformations.quaternion_from_matrix(
align_trans_matrix)
align_qnt_stamped = QuaternionStamped()
align_qnt_stamped.header.frame_id = object_world_pose.header.frame_id
align_qnt_stamped.quaternion.x, align_qnt_stamped.quaternion.y, \
align_qnt_stamped.quaternion.z, align_qnt_stamped.quaternion.w = align_qtn_array
return align_qnt_stamped
def __init__(self, number):
# self.vehicle_number = rospy.get_param('vehicle_number')
self.vehicle_number = number
self.orientation = 0.0
self.orientation_ref = 0.0
self.steer_angle = QuaternionStamped()
self.velocity = 0.0
self.orientation_vel = 0.0
self.linear_vel = 0.0
KP = rospy.get_param('ackermann_control/pid_controller/ori_kp')
KI = rospy.get_param('ackermann_control/pid_controller/ori_ki')
KD = rospy.get_param('ackermann_control/pid_controller/ori_kd')
TS = rospy.get_param('ackermann_control/pid_controller/ori_ts')
ETOL = rospy.get_param('ackermann_control/pid_controller/ori_etol')
self.controlador = PID(KP, KI, KD, TS, ETOL)
rospy.init_node('orientation_PID_control_sim_' +
str(self.vehicle_number),
anonymous=True)
rospy.Subscriber("/odom", Odometry, self.callback_velocity)
rospy.Subscriber("/imu_data", Imu, self.callback_imu)
rospy.Subscriber("/yaw_angle", QuaternionStamped,
self.callback_reference)
#rospy.Subscriber("/cmd_vel", TwistStamped, self.callback_reference)
#rospy.Subscriber("/cmd_vel", Twist, self.callback_reference)
self.pub_angle = rospy.Publisher('/cmd_steer_' +
str(self.vehicle_number),
QuaternionStamped,
queue_size=1)
def get_lines(self, fmt):
t = Time()
t.secs = 0
# MESSAGE_TO_TUM_SHORT
line = ROSMsg2CSVLine.to(fmt, Point(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
line = ROSMsg2CSVLine.to(fmt, PointStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED)
line = ROSMsg2CSVLine.to(fmt, Vector3(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
line = ROSMsg2CSVLine.to(fmt, Vector3Stamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED)
line = ROSMsg2CSVLine.to(
fmt, PoseWithCovarianceStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED)
line = ROSMsg2CSVLine.to(
fmt, PoseWithCovariance(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE)
line = ROSMsg2CSVLine.to(fmt, PoseStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED)
line = ROSMsg2CSVLine.to(fmt, Pose(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSE)
line = ROSMsg2CSVLine.to(fmt, Quaternion(), t,
ROSMessageTypes.GEOMETRY_MSGS_QUATERNION)
line = ROSMsg2CSVLine.to(
fmt, QuaternionStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED)
line = ROSMsg2CSVLine.to(fmt, Transform(), t,
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM)
line = ROSMsg2CSVLine.to(
fmt, TransformStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED)
return line
def main():
global ser, pub
while 1:
msg = ser.readline().split(",");
print "Compass line:", msg
degrees = float(msg[1])
radians = degrees * math.pi / 180.0 + math.pi / 2.0 # east is 0
radians = -(radians + math.pi)
while radians > math.pi:
radians = radians - 2*math.pi
while radians < -math.pi:
radians = radians + 2*math.pi
(qx, qy, qz, qw) = quaternion_from_euler(0,0,radians)
msg = QuaternionStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'base_link'
msg.quaternion.x = qx
msg.quaternion.y = qy
msg.quaternion.z = qz
msg.quaternion.w = qw
pub.publish(msg)
def reset_vars(self):
self.imu_msg = Imu()
self.imu_msg.orientation_covariance = (-1., ) * 9
self.imu_msg.angular_velocity_covariance = (-1., ) * 9
self.imu_msg.linear_acceleration_covariance = (-1., ) * 9
self.pub_imu = False
self.raw_gps_msg = NavSatFix()
self.pub_raw_gps = False
self.pos_gps_msg = NavSatFix()
self.pub_pos_gps = False
self.imu_msg_dq = Imu()
self.imu_msg_dq.orientation_covariance = (-1., ) * 9
self.imu_msg_dq.angular_velocity_covariance = (-1., ) * 9
self.imu_msg_dq.linear_acceleration_covariance = (-1., ) * 9
self.pub_imu_dq = False
self.delta_q_msg = QuaternionStamped()
self.pub_delta_q = False
self.vel_msg = TwistStamped()
self.pub_vel = False
self.mag_msg = MagneticField()
self.mag_msg.magnetic_field_covariance = (0, ) * 9
self.pub_mag = False
self.temp_msg = Temperature()
self.temp_msg.variance = 0.
self.pub_temp = False
self.press_msg = FluidPressure()
self.press_msg.variance = 0.
self.pub_press = False
self.anin1_msg = UInt16()
self.pub_anin1 = False
self.anin2_msg = UInt16()
self.pub_anin2 = False
self.ecef_msg = PointStamped()
self.pub_ecef = False
self.pub_diag = False
def publish_vehicle_attitude(self):
"""Function for publishing the vehicle attitude
"""
#get pose from airsim relative (NED -> FRD)
pose_NED_FRD = self.client.simGetVehiclePose(vehicle_name='drone')
#extract quaternion components
w = pose_NED_FRD.orientation.w_val
x = pose_NED_FRD.orientation.x_val
y = pose_NED_FRD.orientation.y_val
z = pose_NED_FRD.orientation.z_val
#extract rotation matrix
R_NED_FRD = tf.transformations.quaternion_matrix([x, y, z, w])
#convert rotation to (NWU -> FLU)
R_NWU_FLU = self.T_rot.dot(R_NED_FRD).dot(self.T_rot)
#convert to quaternion
q_NWU_FLU = tf.transformations.quaternion_from_matrix(R_NWU_FLU)
#publish attitude
msg = QuaternionStamped()
msg.quaternion.x = q_NWU_FLU[0]
msg.quaternion.y = q_NWU_FLU[1]
msg.quaternion.z = q_NWU_FLU[2]
msg.quaternion.w = q_NWU_FLU[3]
self.vehicle_attitude_pub.publish(msg)
#get euler angles
euler_angles = tf.transformations.euler_from_matrix(R_NWU_FLU, 'ryxz')
return euler_angles[0], euler_angles[1], euler_angles[2]
def angle_head(self,req):
roll = 0
yaw = req.pan
pitch = req.tilt
(qx,qy,qz,qw) = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
final_quat = QuaternionStamped()
final_quat.header.frame_id = "torso_lift_link"
final_quat.header.stamp = rospy.Time.now()
final_quat.quaternion.x = qx
final_quat.quaternion.y = qy
final_quat.quaternion.z = qz
final_quat.quaternion.w = qw
#Convert the angle to the current stereo frame
self.listener.waitForTransform("double_stereo_link","torso_lift_link",rospy.Time.now(),rospy.Duration(10.0))
rel_quat = self.listener.transformQuaternion("double_stereo_link",final_quat)
rel_quat_tuple = (rel_quat.quaternion.x,rel_quat.quaternion.y,rel_quat.quaternion.z,rel_quat.quaternion.w)
#Get the point which corresponds to one meter ahead of the stereo frame
straight_target_q = (1.0,0,0,0)
#Rotate by the quaternion
(x,y,z,trash) = tf.transformations.quaternion_multiply(rel_quat_tuple,tf.transformations.quaternion_multiply(straight_target_q,tf.transformations.quaternion_inverse(rel_quat_tuple)))
new_target = PointStamped()
new_target.header.stamp = rospy.Time.now()
new_target.header.frame_id = "double_stereo_link"
new_target.point.x = x
new_target.point.y = y
new_target.point.z = z
#Now put in torso_lift_link frame
self.listener.waitForTransform("torso_lift_link","double_stereo_link",rospy.Time.now(),rospy.Duration(10.0))
target = self.listener.transformPoint("torso_lift_link",new_target)
#self.follow_target = target
success = self.look_at(target)
self.follow_target = False
return AngleHeadResponse(success)
def __init__(self):
self._quaternion = QuaternionStamped()
self._quaternion.quaternion.x = 0.0
self._quaternion.quaternion.y = 0.0
self._quaternion.quaternion.z = 0.0
self._quaternion.quaternion.w = 1.0
bg_color_str = rospy.get_param('~bg_color', "255,255,255")
self.bg_color_set = list()
for item in bg_color_str.split(","):
self.bg_color_set.append(int(item))
self.hz = rospy.get_param('~hz', 30)
rospy.Subscriber("src_topic", QuaternionStamped,
self.quaternion_callback)
# pygame
pygame.init()
if rospy.get_param('~is_fullscreen', "false"):
cmd1 = ['xrandr']
cmd2 = ['grep', '*']
resolution_string, _ = self._command2pipe(cmd1, cmd2)
resolution = resolution_string.split()[0]
width, height = resolution.split('x')
self.screen_size = (int(width), int(height))
self.screen = pygame.display.set_mode(self.screen_size, FULLSCREEN,
32)
else:
self.screen_size = (rospy.get_param('~width', 800),
rospy.get_param('~height', 480))
self.screen = pygame.display.set_mode(self.screen_size)
pygame.display.set_caption("roseyes")
# pygame
self.update_loop()
def serialize(self, data):
msg = QuaternionStamped()
msg.header.stamp.sec = data.tm.sec
msg.header.stamp.nanosec = data.tm.nsec
msg.quaternion.x = float(data.data.x)
msg.quaternion.y = float(data.data.y)
msg.quaternion.z = float(data.data.z)
msg.quaternion.w = float(data.data.w)
return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_OK, msg
def serialize(self, data):
msg = QuaternionStamped()
msg.header.stamp.secs = data.tm.sec
msg.header.stamp.nsecs = data.tm.nsec
msg.quaternion.x = data.data.x
msg.quaternion.y = data.data.y
msg.quaternion.z = data.data.z
msg.quaternion.w = data.data.w
buf = ros_serialize(msg)
return OpenRTM_aist.ByteDataStreamBase.SERIALIZE_OK, buf
def from_TUM(line, line_number, msg_type):
msg = None
s = CSVFormatPose.parse(line, CSVFormatPose.TUM)
h = Header()
h.stamp = h.stamp.from_sec(s.t)
h.seq = int(line_number)
if msg_type == ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED:
msg = PointStamped()
msg.header = h
msg.point = Point(x=s.tx, y=s.ty, z=s.tz)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_VECTOR3:
msg = Vector3(x=s.tx, y=s.ty, z=s.tz)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED:
msg = Vector3Stamped()
msg.header = h
msg.vector = Vector3(x=s.tx, y=s.ty, z=s.tz)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED:
msg = PoseWithCovarianceStamped()
msg.header = h
msg.pose.pose.position = Point(x=s.tx, y=s.ty, z=s.tz)
msg.pose.pose.orientation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE:
msg = PoseWithCovariance()
msg.pose.position = Point(x=s.tx, y=s.ty, z=s.tz)
msg.pose.orientation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED:
msg = PoseStamped()
msg.header = h
msg.pose.position = Point(x=s.tx, y=s.ty, z=s.tz)
msg.pose.orientation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_POSE:
msg = Pose()
msg.position = Point(x=s.tx, y=s.ty, z=s.tz)
msg.orientation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_QUATERNION:
msg = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED:
msg = QuaternionStamped()
msg.header = h
msg.quaternion = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM:
msg = Transform()
msg.translation = Vector3(x=s.tx, y=s.ty, z=s.tz)
msg.rotation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
elif msg_type == ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED:
msg = TransformStamped()
msg.header = h
msg.transform.translation = Vector3(x=s.tx, y=s.ty, z=s.tz)
msg.transform.rotation = Quaternion(x=s.qx, y=s.qy, z=s.qz, w=s.qw)
# else:
return msg, s.t
def rot_cb(self, ctx, data):
temp_data = parse_value(data)
quat = QuaternionStamped()
quat.header.stamp = rospy.Time.now()
quat.header.frame_id = self.frame_id
quat.quaternion.x = temp_data.x
quat.quaternion.y = temp_data.y
quat.quaternion.z = temp_data.z
quat.quaternion.w = temp_data.w
self.samples_rot += 1
self.pub_rotation.publish(quat)
def publishData(self):
head = Header()
head.stamp = rospy.Time.now()
head.frame_id = 'local'
# Publish Attitude
myQuat = self.getQuat()
outQuat = QuaternionStamped()
outQuat.header = head
outQuat.quaternion = myQuat
self.attitude_pub_.publish(outQuat)
# Publish IMU
imuOut = Imu()
imuOut.header = head
imuOut.orientation = myQuat
imuOut.angular_velocity = Vector3(x=0.0, y=0.0, z=self.yaw_rate_)
imuOut.linear_acceleration = Vector3(x=self.acc_[0],
y=self.acc_[1],
z=self.acc_[2] + 9.81)
self.imu_pub_.publish(imuOut)
# Publish Velocity
velOut = Vector3Stamped()
velOut.header = head
velOut.vector = Vector3(x=self.vel_[0], y=self.vel_[1], z=self.vel_[2])
self.velocity_pub_.publish(velOut)
# Publish Position
posStamp = PointStamped()
posStamp.header = head
myPoint = Point(x=self.pos_[0], y=self.pos_[1], z=self.pos_[2])
posStamp.point = myPoint
self.position_pub_.publish(posStamp)
# Publish Height Above Ground
height = Float32(self.pos_[-1])
# height.data = self.pos_(-1)
self.height_pub_.publish(height)
def _link_state_callback(self, msg):
for i in range(0, len(msg.name)):
if (msg.name[i] == "robostilt::base_link"):
orientation_q = msg.pose[i].orientation
# publish level info to topic
message = QuaternionStamped()
message.header.frame_id = "base_link"
message.header.stamp = rospy.Time.now()
message.quaternion = orientation_q
self.publisher.publish(message)
# Calculate euler angles
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z,
orientation_q.w
]
(roll_x, pitch_y, yaw_z
) = tf.transformations.euler_from_quaternion(orientation_list)
if (abs(roll_x) > self.max_level_error
or abs(pitch_y) > self.max_level_error):
rospy.logerr("Level is out of range. Roll_x= " +
str(roll_x * self.rad_to_deg) + " Pitch_y= " +
str(pitch_y * self.rad_to_deg))
def __init__(self):
rospy.init_node('dji_sdk_ros_python')
self.attitude = QuaternionStamped()
self.flight_status = UInt8()
self.battery_state = BatteryState()
self.velocity = Vector3Stamped()
self.gps_health = UInt8()
self.gps_position = NavSatFix()
self.local_position = PointStamped()
self.init_services()
self.init_subscribers()
self.init_publishers()
def test_MESSAGE_TO_TUM(self):
t = Time()
t.secs = 0
self.check_tumline(
ROSMsg2CSVLine.to_TUM(Point(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(PointStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(Vector3(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(
Vector3Stamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(
PoseWithCovarianceStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(
PoseWithCovariance(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(PoseStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(Pose(), t,
ROSMessageTypes.GEOMETRY_MSGS_POSE))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(Quaternion(), t,
ROSMessageTypes.GEOMETRY_MSGS_QUATERNION))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(
QuaternionStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(Transform(), t,
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM))
self.check_tumline(
ROSMsg2CSVLine.to_TUM(
TransformStamped(), t,
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED))
def get_orientation(self, from_frame="wrist_left_ft_link"):
qs = QuaternionStamped()
qs.quaternion.w = 1.0
qs.header.stamp = self.tf_l.getLatestCommonTime("world", from_frame)
qs.header.frame_id = "/" + from_frame
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
world_q = self.tf_l.transformQuaternion("/world", qs)
transform_ok = True
return world_q
except ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming pose... trying again \n(" +
str(e) + ")")
rospy.sleep(0.05)
qs.header.stamp = self.tf_l.getLatestCommonTime(
"world", from_frame)
def main():
global ser, pub
while 1:
msg = ser.readline().split(",");
yaw = float(msg[1]);
pitch = float(msg[2]);
roll = float(msg[3]);
#out = quaternion_from_euler(roll,pitch,yaw)#,'rxyz')
out = quaternion_from_euler(0,0,yaw)#,'rxyz')
msg = QuaternionStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'base_link'
msg.quaternion.x = out[0]
msg.quaternion.y = out[1]
msg.quaternion.z = out[2]
msg.quaternion.w = out[3]
pub.publish(msg)
def write_ms25_euler(ms25_euler, i, bag):
utime = ms25_euler[i, 0]
r = ms25_euler[i, 1]
p = ms25_euler[i, 2]
h = ms25_euler[i, 3]
timestamp = microseconds_to_ros_timestamp(utime)
rosquat = QuaternionStamped()
rosquat.header.stamp = timestamp
q = getQuaternionFromFromEulerAnglesRollPitchYawRad(r, p, h)
rosquat.quaternion.x = q.x()
rosquat.quaternion.y = q.y()
rosquat.quaternion.z = q.z()
rosquat.quaternion.w = q.w()
bag.write('/ms25_orientation', rosquat, t=timestamp)
def transform_rotation(rotation, from_frame, to_frame):
if from_frame is to_frame:
return rotation
global transform_point_listener
if transform_point_listener is None:
transform_point_listener = tf.TransformListener()
rate = rospy.Rate(1.0)
return_list = True
if not isinstance(rotation, list):
rotation = [rotation]
return_list = False
print("Looking up transform from " + from_frame + " to " + to_frame +
" for " + str(len(rotation)) + " rotations.")
ret = []
for r in rotation:
while True:
try:
hdr = Header(stamp=rospy.Time(0), frame_id=from_frame)
msg = QuaternionStamped(header=hdr, quaternion=r)
retmsg = transform_point_listener.transformQuaternion(
to_frame, msg)
except (tf.LookupException):
print("lookup excpetion")
rate.sleep()
continue
except (tf.ExtrapolationException):
print("extrapolation excpetion")
rate.sleep()
continue
break
ret.append(retmsg.quaternion)
if return_list:
return ret
else:
return ret[0]
def test_get_message_type(self):
self.assertTrue(
ROSMessageTypes.get_message_type(Point()) ==
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
self.assertTrue(
ROSMessageTypes.get_message_type(PointStamped()) ==
ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED)
self.assertTrue(
ROSMessageTypes.get_message_type(Vector3()) ==
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3)
self.assertTrue(
ROSMessageTypes.get_message_type(Vector3Stamped()) ==
ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED)
self.assertTrue(
ROSMessageTypes.get_message_type(PoseWithCovarianceStamped()) ==
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED)
self.assertTrue(
ROSMessageTypes.get_message_type(PoseWithCovariance()) ==
ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE)
self.assertTrue(
ROSMessageTypes.get_message_type(PoseStamped()) ==
ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED)
self.assertTrue(
ROSMessageTypes.get_message_type(Pose()) ==
ROSMessageTypes.GEOMETRY_MSGS_POSE)
self.assertTrue(
ROSMessageTypes.get_message_type(Quaternion()) ==
ROSMessageTypes.GEOMETRY_MSGS_QUATERNION)
self.assertTrue(
ROSMessageTypes.get_message_type(QuaternionStamped()) ==
ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED)
self.assertTrue(
ROSMessageTypes.get_message_type(Transform()) ==
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM)
self.assertTrue(
ROSMessageTypes.get_message_type(TransformStamped()) ==
ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED)
def pub_timer_callback(event):
"""Periodic callback function to publish the current orientation of the
gimbal. This function will terminate the ROS node if it ecounters a
SerialException.
Arg:
event: ROS event object for the periodic callback.
"""
# Create new message and header
msg = QuaternionStamped()
msg.header.frame_id = frame_id
msg.header.stamp = rospy.get_rostime()
try:
imu1 = (gimbal.get_imu1_angles(), camera_pub, "imu1")
imu2 = (gimbal.get_imu2_angles(), controller_pub, "imu2")
for euler, pub, name in (imu1, imu2):
if euler:
rospy.logdebug_throttle(
1.0,
"get_{0}_angles: pitch={1:.2f}, roll={2:.2f}, yaw={3:.2f}".
format(name, *euler))
euler = list(map(np.radians, euler))
q = tf.transformations.quaternion_from_euler(*euler,
axes="syxz")
msg.quaternion.x = q[0]
msg.quaternion.y = q[1]
msg.quaternion.z = q[2]
msg.quaternion.w = q[3]
pub.publish(msg)
# diagnostic update is called here because this run at a high rate
updater.update()
except serial.serialutil.SerialException as e:
rospy.logfatal(e)
rospy.signal_shutdown("SerialError: {0:s}".format(e))
def inputcb(self, data):
rospy.logdebug("inputcb triggered")
# translation from base_link to left string expressed in the
# base_link frame:
vb = np.array([[0.0102, 0.0391, 0.086, 0]]).T
gbs = np.array([[1, 0, 0, vb[0,0]],
[0, 0, -1, vb[1,0]],
[0, 1, 0, vb[2,0]],
[0, 0, 0, 1]])
gsb = np.linalg.inv(gbs)
# map to base stuff:
pbm = np.array([[data.pose.pose.position.x,
data.pose.pose.position.y,
data.pose.pose.position.z]]).T
qbm = np.array([[data.pose.pose.orientation.w,
data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z]]).T
Rbm = quat_to_rot(qbm)
gbm = to_se3(Rbm,pbm)
gmb = np.linalg.inv(gbm)
vm = np.dot(gmb,np.dot(gbs,np.dot(gsb,vb)))
vm = np.ravel(vm)[0:3]
## so the first thing that we need to do is get the location
## of the robot in the "/optimization_frame"
p = PointStamped()
p.header = data.pose.header
p.point = data.pose.pose.position
p.point.x -= vm[0]
p.point.y -= vm[1]
p.point.z -= vm[2]
quat = QuaternionStamped()
quat.quaternion = data.pose.pose.orientation
quat.header = p.header
try:
ptrans = self.listener.transformPoint("/optimization_frame", p)
qtrans = self.listener.transformQuaternion("/optimization_frame", quat)
except (tf.Exception):
rospy.loginfo("tf exception caught !")
return
## if we have not initialized the VI, let's do it, otherwise,
## let's integrate
if not self.initialized_flag:
q = [
ptrans.point.x,
ptrans.point.y-h0,
ptrans.point.z,
ptrans.point.x,
ptrans.point.z,
h0 ]
self.sys.reset_integration(state=q)
self.last_time = ptrans.header.stamp
self.initialized_flag = True
return
# get operating_condition:
if rospy.has_param("/operating_condition"):
operating = rospy.get_param("/operating_condition")
else:
return
# set string length
self.len = data.left;
## if we are not running, just reset the parameter:
if operating is not 2:
self.initialized_flag = False
return
else:
self.initialized_flag = True
# if we are in the "running mode", let's integrate the VI,
# and publish the results:
rho = [ptrans.point.x, ptrans.point.z, self.len]
dt = (ptrans.header.stamp - self.last_time).to_sec()
self.last_time = ptrans.header.stamp
rospy.logdebug("Taking a step! dt = "+str(dt))
self.sys.take_step(dt, rho)
## now we can get the state of the system
q = self.sys.get_current_configuration()
## now add the appropriate amount of noise:
q += self.noise*np.random.normal(0,1,(self.sys.sys.nQ))
new_point = PointStamped()
new_point.point.x = q[0]
new_point.point.y = q[1]
new_point.point.z = q[2]
new_point.header.frame_id = "/optimization_frame"
new_point.header.stamp = rospy.rostime.get_rostime()
rospy.logdebug("Publishing mass location")
self.mass_pub.publish(new_point)
## we can also publish the planar results:
config = PlanarSystemConfig()
config.header.frame_id = "/optimization_frame"
config.header.stamp = rospy.get_rostime()
config.xm = q[0]
config.ym = q[1]
config.xr = rho[0]
config.r = rho[2]
self.plan_pub.publish(config)
## now we can send out the transform
ns = rospy.get_namespace()
fr = "mass_location"
if len(ns) > 1:
fr = rospy.names.canonicalize_name(ns+fr)
# here we are going to do a bunch of geometry math to
# ensure that the orientation of the frame that we are
# placing at the mass location looks "nice"
# zvec points from robot to the string
zvec = np.array([q[0]-ptrans.point.x, q[1]-ptrans.point.y, q[2]-ptrans.point.z])
zvec = zvec/np.linalg.norm(zvec)
# get transform from incoming header frame to the optimization frame
quat = qtrans.quaternion
qtmp = np.array([quat.x, quat.y, quat.z, quat.w])
# convert to SO(3), and extract the y-vector for the frame
R1 = tf.transformations.quaternion_matrix(qtmp)[:3,:3]
yvec = -R1[:,1]
yvec[1] = (-yvec[0]*zvec[0]-yvec[2]*zvec[2])/zvec[1]
# x vector is a cross of y and z
xvec = np.cross(yvec, zvec)
# build rotation matrix and send transform
R = np.column_stack((xvec,yvec,zvec,np.array([0,0,0])))
R = np.row_stack((R,np.array([0,0,0,1])))
quat = tuple(tf.transformations.quaternion_from_matrix(R).tolist())
self.br.sendTransform((new_point.point.x, new_point.point.y, new_point.point.z),
quat,
new_point.header.stamp,
fr,
"/optimization_frame")
return
