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(