def _broadcastTransform(self, Pose, parentName, childName):
br = TransformBroadcaster()
tr = TransformStamped()
tr.header.stamp = Time.now()
tr.header.frame_id = parentName
tr.child_frame_id = childName
tr.transform.translation = Pose.position
tr.transform.rotation = Pose.orientation
br.sendTransform(tr)
def publish(self):
now = self.get_clock().now()
self.odometry.updatePose(now)
pose = self.odometry.getPose()
#q = quaternion_from_euler(0, 0, pose.theta)
#self.tfPub.sendTransform(
# (pose.x, pose.y, 0),
# (q[0], q[1], q[2], q[3]),
# now,
# self.baseFrameID,
# self.odomFrameID
#)
# Translate it
odom_trans = TransformStamped()
odom_trans.header.stamp = now.to_msg()
odom_trans.header.frame_id = self.odomFrameID
odom_trans.child_frame_id = self.baseFrameID
odom_trans.transform.translation.x = float(pose.x)
odom_trans.transform.translation.y = float(pose.y)
odom_trans.transform.translation.z = 0.0
self.tfPub.sendTransform(odom_trans)
#odom = Odometry()
#odom.header.stamp = now
#odom.header.frame_id = self.odomFrameID
#odom.child_frame_id = self.baseFrameID
#odom.pose.pose.position.x = pose.x
#odom.pose.pose.position.y = pose.y
#odom.pose.pose.orientation.x = q[0]
#odom.pose.pose.orientation.y = q[1]
#odom.pose.pose.orientation.z = q[2]
#odom.pose.pose.orientation.w = q[3]
#odom.twist.twist.linear.x = pose.xVel
#odom.twist.twist.angular.z = pose.thetaVel
#self.odomPub.publish(odom)
odom = Odometry()
odom.header.stamp = now.to_msg()
odom.header.frame_id = self.odomFrameID
odom.child_frame_id = self.baseFrameID
odom.pose.pose.position.x = pose.x
odom.pose.pose.position.y = pose.y
odom.pose.pose.position.z = 0.0
quaternion = Quaternion(x=0.0,
y=0.0,
z=sin(pose.theta / 2.0),
w=cos(pose.theta / 2.0))
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = pose.xVel
odom.twist.twist.linear.y = 0.0
odom.twist.twist.angular.z = pose.thetaVel
self.odomPub.publish(odom)
def state_callback(self, msg):
self.pose_array.append(msg)
now = self.get_clock().now()
p = msg.pose.position
q = msg.pose.orientation
odom_trans = TransformStamped()
odom_trans.header.frame_id = msg.header.frame_id
odom_trans.child_frame_id = 'base_link'
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = p.x
odom_trans.transform.translation.y = p.y
odom_trans.transform.translation.z = p.z
odom_trans.transform.rotation = q
self.broadcaster.sendTransform(odom_trans)
path_msg = Path()
path_msg.header.frame_id = msg.header.frame_id
path_msg.header.stamp = now.to_msg()
path_msg.poses = self.pose_array
self.path_publisher.publish(path_msg)
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states',
qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
loop_rate = self.create_rate(30)
# robot state
steering_angle = 0.
wheel_angle = 0.
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'base_link'
joint_state = JointState()
try:
t = 0
while rclpy.ok():
t += 1. / 30.
self.get_logger().info(str(t))
rclpy.spin_once(self)
# Set angle
rotation_names = [
'front_left_wheel_joint', 'front_right_wheel_joint',
'rear_right_wheel_joint', 'rear_left_wheel_joint'
]
wheel_angle += 2 * pi * t / 50
wheel_angle = atan2(sin(wheel_angle), cos(wheel_angle))
rotation_position = [
wheel_angle, wheel_angle, wheel_angle, wheel_angle
]
# Set steering
steering_angle = pi / 3. * sin(t * 2 * pi / 4.)
steering_names = [
'front_left_steering_joint', 'front_right_steering_joint'
]
steering_position = [steering_angle, steering_angle]
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = rotation_names + steering_names
joint_state.position = rotation_position + steering_position
# update transform
# (moving in a circle with radius=2)
angle = t * 2 * pi / 10.
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = cos(angle) * 2
odom_trans.transform.translation.y = sin(angle) * 2
odom_trans.transform.translation.z = 0.7
odom_trans.transform.rotation = \
euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
# send the joint state and transform
self.joint_pub.publish(joint_state)
self.broadcaster.sendTransform(odom_trans)
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
def __init__(self):
rclpy.init()
super().__init__("state_publisher")
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, "joint_states", qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
degree = pi / 180.0
loop_rate = self.create_rate(30)
# robot state
tilt = 0.0
tinc = degree
swivel = 0.0
angle = 0.0
height = 0.0
hinc = 0.005
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = "odom"
odom_trans.child_frame_id = "base_link"
joint_state = JointState()
joint_names = [
"joint_tracks_gantry",
"joint_gantry_crossSlide",
"joint_cross_slide_z_axis",
"joint_gantry_crossSlide2",
]
default_position = [0.0, 0.0, 0.0, 0.0]
try:
while rclpy.ok():
rclpy.spin_once(self)
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = joint_names
joint_state.position = default_position
"""
# update transform
# (moving in a circle with radius=2)
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = cos(angle) * 2
odom_trans.transform.translation.y = sin(angle) * 2
odom_trans.transform.translation.z = 0.7
odom_trans.transform.rotation = euler_to_quaternion(
0, 0, angle + pi / 2
) # roll,pitch,yaw
"""
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = 0.0
odom_trans.transform.translation.y = 0.0
odom_trans.transform.translation.z = 0.0
odom_trans.transform.rotation = euler_to_quaternion(0, 0, 0)
# send the joint state and transform
self.joint_pub.publish(joint_state)
self.broadcaster.sendTransform(odom_trans)
# Create new robot state
"""
tilt += tinc
if tilt < -0.5 or tilt > 0.0:
tinc *= -1
height += hinc
if height > 0.2 or height < 0.0:
hinc *= -1
swivel += degree
angle += degree / 4
"""
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
degree = pi / 180.0
loop_rate = self.create_rate(30)
# robot state
tilt = 0.
tinc = degree
swivel = 0.
angle = 0.
height = 0.
hinc = 0.005
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'axis'
joint_state = JointState()
try:
while rclpy.ok():
rclpy.spin_once(self)
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = ['swivel', 'tilt', 'periscope']
joint_state.position = [swivel, tilt, height]
# update transform
# (moving in a circle with radius=2)
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = cos(angle)*2
odom_trans.transform.translation.y = sin(angle)*2
odom_trans.transform.translation.z = 0.7
odom_trans.transform.rotation = \
euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
# send the joint state and transform
self.joint_pub.publish(joint_state)
self.broadcaster.sendTransform(odom_trans)
# Create new robot state
tilt += tinc
if tilt < -0.5 or tilt > 0.0:
tinc *= -1
height += hinc
if height > 0.2 or height < 0.0:
hinc *= -1
swivel += degree
angle += degree/4
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
def fast_callback(self, event):
""" Callback function that requests Waterlinked position and orientation
information at a fast rate. """
# Request current time and use it for all messages
tnow = rospy.Time.now()
if self._is_first_fast_loop:
self._is_first_fast_loop = False
self.f_cum_fast = 0
self.n_fast = 0
self._fast_t0 = tnow.to_sec()
else:
f = 1 / (tnow.to_sec() - self._fast_t0)
self.f_cum_fast += f
self.n_fast += 1
f_avg = self.f_cum_fast / self.n_fast
rospy.logdebug("fast loop (n = %d): f_avg = %.3f Hz" %
(self.n_fast, f_avg))
# Initiate HTTP request to all URLs
future_list = [
self._session.get(url, timeout=2.0) for url in self._urls_fast
]
try:
# WARN: ORIENTATION IS CLOCKWISE REFERENCED FROM MAGNETIC NORTH
# /waterlinked/external/orientation
res_external_orientation = future_list[0].result()
if res_external_orientation.ok:
data = res_external_orientation.json()
msg_external_orientation = ExternalOrientation()
msg_external_orientation.header.stamp = tnow
msg_external_orientation.orientation = data['orientation']
self._pub_external_orientation.publish(
msg_external_orientation)
# /waterlinked/position/acoustic/filtered
res_position_acoustic_filtered = future_list[1].result()
# WARN: WATERLINKED POSITION IS LEFT HANDED RFD -> X: RIGHT, y: FORWARDS, Z: DOWN
# DO NOT USE ACOUSTIC_FILTERED FOR NAVIGATION!
if res_position_acoustic_filtered.ok:
data = res_position_acoustic_filtered.json()
msg_position_acoustic_filtered = PositionAcousticFiltered()
msg_position_acoustic_filtered.header.stamp = tnow
msg_position_acoustic_filtered.header.frame_id = self._waterlinked_frame_id
msg_position_acoustic_filtered.std = data['std']
msg_position_acoustic_filtered.temp = data['temp']
msg_position_acoustic_filtered.x = data['x']
msg_position_acoustic_filtered.y = data['y']
msg_position_acoustic_filtered.z = data['z']
if self._pub_position_acoustic_filtered.get_num_connections(
) > 0:
rospy.logwarn_once(
"{} | waterlinked/acoustic_filtered is left-handed RFD, don't use for navigation, "
"use waterlinked/pose_with_cov_stamped (FLU) instead.")
self._pub_position_acoustic_filtered.publish(
msg_position_acoustic_filtered)
# Create message of the type geometry_msgs/PoseWithCovariance
msg_pose_with_cov_stamped = PoseWithCovarianceStamped()
var_xyz = pow(data['std'],
2) # calculate variance from standard deviation
msg_pose_with_cov_stamped.header.stamp = tnow
msg_pose_with_cov_stamped.header.frame_id = self._waterlinked_frame_id
msg_pose_with_cov_stamped.pose.pose.position.x = data['y']
msg_pose_with_cov_stamped.pose.pose.position.y = -data['x']
msg_pose_with_cov_stamped.pose.pose.position.z = -data['z']
msg_pose_with_cov_stamped.pose.pose.orientation = Quaternion(
0, 0, 0, 1)
msg_pose_with_cov_stamped.pose.covariance = [
var_xyz, 0, 0, 0, 0, 0, 0, var_xyz, 0, 0, 0, 0, 0, 0,
var_xyz, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
]
self._pub_pos_with_covariance_stamped.publish(
msg_pose_with_cov_stamped)
# /waterlinked/position/acoustic/raw
res_position_acoustic_raw = future_list[2].result()
if res_position_acoustic_raw.ok:
data = res_position_acoustic_raw.json()
msg_position_acoustic_raw = PositionAcousticRaw()
msg_position_acoustic_raw.header.stamp = tnow
msg_position_acoustic_raw.header.frame_id = self._waterlinked_frame_id
msg_position_acoustic_raw.std = data['std']
msg_position_acoustic_raw.temp = data['temp']
msg_position_acoustic_raw.x = data['x']
msg_position_acoustic_raw.y = data['y']
msg_position_acoustic_raw.z = data['z']
if self._pub_position_acoustic_raw.get_num_connections() > 0:
rospy.logwarn_once(
"{} | waterlinked/acoustic_raw is left-handed RFD, don't use for navigation, "
"use waterlinked/pose_with_cov_stamped (FLU) instead.")
self._pub_position_acoustic_raw.publish(
msg_position_acoustic_raw)
# /waterlinked/position/global
res_position_global = future_list[3].result()
if res_position_global.ok:
data = res_position_global.json()
msg_position_global = PositionGlobal()
msg_position_global.header.stamp = tnow
msg_position_global.lat = data['lat']
msg_position_global.lon = data['lon']
self._pub_position_global.publish(msg_position_global)
# /waterlinked/position/master
res_position_master = future_list[4].result()
msg_position_master = None
if res_position_master.ok:
data = res_position_master.json()
msg_position_master = PositionMaster()
msg_position_master.header.stamp = tnow
msg_position_master.cog = data['cog']
msg_position_master.hdop = data['hdop']
msg_position_master.lat = data['lat']
msg_position_master.lon = data['lon']
msg_position_master.numsats = data['numsats']
msg_position_master.orientation = data['orientation']
msg_position_master.sog = data['sog']
self._pub_position_master.publish(msg_position_master)
# CONVENTION: UTM -> WATERLINKED IS DEFINED BY UTM POSITION OF MASTER, ROTATED ACCORDING TO MASTER ORIENTATION
# CONVENTION: UTM -> MAP IS DEFINED BY UTM POSITION OF MASTER, WITHOUT ANY ROTATION (ALIGNED WITH NORTH)
# CONVENTION: UTM -> MAP CAN ALSO BE DEFINED BY AN EXTERNAL DATUM [LATITUDE, LONGITUDE]
if self._send_tf and msg_position_master is not None:
tf_map = TransformStamped() # Map transformation
tf_map.header.stamp = tnow
tf_map.header.frame_id = self._map_frame_id
tf_map.child_frame_id = self._waterlinked_frame_id
# ORIENTATION IS PROVIDED AS NORTH REFERENCED CW
# NEEDS TO BE CONVERTED TO EAST REFERENCED CCW
q = Rotation.from_euler(
'xyz', [0, 0, 90 - msg_position_master.orientation],
degrees=True).as_quat()
tf_map.transform.rotation = Quaternion(*q)
self._tf_bcast.sendTransform(tf_map)
except ConnectionError as e:
self.connection_error()
elif args.camera is False and args.video is None:
raise ValueError(
"Missing inference source! Either use \"-cam\" to run on DepthAI camera or \"-vid <path>\" to run on video file"
)
rclpy.init(args=None)
node = rclpy.create_node('head_orientation')
qos_profile = QoSProfile(depth=10)
broadcaster = TransformBroadcaster(node, qos=qos_profile)
# Transform declaration
transform = TransformStamped()
transform.header.frame_id = 'oak-d_camera_center'
transform.child_frame_id = 'head'
def pose_to_quaternion(head_pose):
# roll = head_pose[1] * np.pi / 180
# pitch = head_pose[2] * np.pi / 180
# yaw = head_pose[0] * np.pi / 180
roll = -(head_pose[0] * np.pi / 180)
pitch = head_pose[2] * np.pi / 180
yaw = head_pose[1] * np.pi / 180
qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
roll / 2) * np.sin(pitch / 2) * np.sin(yaw / 2)
qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2)
qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(