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 setUp(self): self.point_cloud_in = point_cloud2.PointCloud2() self.point_cloud_in.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] self.point_cloud_in.point_step = 4 * 3 self.point_cloud_in.height = 1 # we add two points (with x, y, z to the cloud) self.point_cloud_in.width = 2 self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width points = [1, 2, 0, 10, 20, 30] self.point_cloud_in.data = struct.pack('%sf' % len(points), *points) self.transform_translate_xyz_300 = TransformStamped() self.transform_translate_xyz_300.transform.translation.x = 300 self.transform_translate_xyz_300.transform.translation.y = 300 self.transform_translate_xyz_300.transform.translation.z = 300 self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w assert (list(point_cloud2.read_points(self.point_cloud_in)) == [ (1.0, 2.0, 0.0), (10.0, 20.0, 30.0) ])
def __init__(self): self._datum = rospy.get_param("~datum", None) if self._datum is not None and len(self._datum) < 3: self._datum += [0.0] self._use_datum = self._datum is not None self._earth_frame_id = rospy.get_param("~earth_frame_id", "earth") self._utm_frame_id = rospy.get_param("~utm_frame_id", "utm") self._map_frame_id = rospy.get_param("~map_frame_id", "map") self._odom_frame_id = rospy.get_param("~odom_frame_id", "odom") self._body_frame_id = rospy.get_param("~base_frame_id", "base_link") # currently unused self._tf_broadcast_rate = rospy.get_param("~broadcast_rate", 10.0) self._serv = rospy.Service("~set_datum", SetDatum, self._set_datum) self._tf_buff = Buffer() TransformListener(self._tf_buff) self._tf_bcast = TransformBroadcaster() self._last_datum = None self._static_map_odom = rospy.get_param("~static_map_odom", False) self._odom_updated = False self._update_odom_serv = rospy.Service("~set_odom", Trigger, self._handle_set_odom) if not self._use_datum: rospy.Subscriber("gps_datum", NavSatFix, self._handle_datum) if not self._static_map_odom: rospy.Subscriber("waterlinked/pose_with_cov_stamped", PoseWithCovarianceStamped, self._handle_odom_tf) else: StaticTransformBroadcaster().sendTransform( TransformStamped( Header(0, rospy.Time.now(), self._map_frame_id), self._odom_frame_id, None, Quaternion(0, 0, 0, 1))) self._map_odom_tf = None rospy.Timer(rospy.Duration.from_sec(1.0 / self._tf_broadcast_rate), self._send_tf)
def setUp(self): self.point_cloud_in = point_cloud2.PointCloud2() self.point_cloud_in.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('index', 12, PointField.INT32, 1) ] self.point_cloud_in.point_step = 4 * 4 self.point_cloud_in.height = 1 # we add two points (with x, y, z to the cloud) self.point_cloud_in.width = 2 self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width self.points = [(1.0, 2.0, 0.0, 123), (10.0, 20.0, 30.0, 456)] for point in self.points: self.point_cloud_in.data += struct.pack('3fi', *point) self.transform_translate_xyz_300 = TransformStamped() self.transform_translate_xyz_300.transform.translation.x = self.TRANSFORM_OFFSET_DISTANCE self.transform_translate_xyz_300.transform.translation.y = self.TRANSFORM_OFFSET_DISTANCE self.transform_translate_xyz_300.transform.translation.z = self.TRANSFORM_OFFSET_DISTANCE self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w assert (list(point_cloud2.read_points( self.point_cloud_in)) == self.points)
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 _get_tfs(self, data): if type(data) is NavSatFix: earth, utm = self._get_coords(data.latitude, data.longitude) header_earth = Header(data.header.seq, data.header.stamp, self._earth_frame_id) header_utm = Header(data.header.seq, data.header.stamp, self._utm_frame_id) else: earth, utm = self._get_coords(self._datum[0], self._datum[1]) header_earth = Header(0, rospy.Time.now(), self._earth_frame_id) header_utm = Header(0, rospy.Time.now(), self._utm_frame_id) # Broadcast datum's latitude and longitude # Map from ECEF Frame earth_to_utm = TransformStamped( header_earth, self._utm_frame_id, Transform(Vector3(*earth[:3]), Quaternion(*earth[3:]))) # UTM from ECEF Frame utm_to_map = TransformStamped( header_utm, self._map_frame_id, Transform(Vector3(*utm[:3]), Quaternion(*utm[3:]))) return earth_to_utm, utm_to_map
def __init__(self): rclpy.init() super().__init__('oakd_publisher') qos_profile = QoSProfile(depth=10) self.broadcaster = TransformBroadcaster(self, qos=qos_profile) #Image publisher #self.pub_depth_img = self.create_publisher(sensor_msgs.msg.Image, "/depth_img", 10) self.pub_rectified_img = self.create_publisher(Image, "/rectified_img", 10) # Transform for head declaration self.transform_head = TransformStamped() self.transform_head.header.frame_id = 'oak-d_camera_center' self.transform_head.child_frame_id = 'head' # Transform for palm declaration self.transform_palm = TransformStamped() self.transform_palm.header.frame_id = 'oak-d_camera_center' self.transform_palm.child_frame_id = 'palm' #fixed Identity quaternion (no rotation) self.fps = FPS() self.advanced_main()
def __init__(self): self.fixed_frame = rospy.get_param( '~fixed_frame', "world" ) # set fixed frame relative to world to apply the transform self.publish_transform = rospy.get_param( '~publish_transform', False) # set true in launch file if the transform is needed self.pub_jnt = rospy.Publisher("joint_states", JointState, queue_size=10) # joint state publisher self.odom_broadcaster = TransformBroadcaster() self.jnt_msg = JointState() # joint topic structure self.odom_trans = TransformStamped() self.odom_trans.header.frame_id = 'world' self.odom_trans.child_frame_id = 'base_link' self.jnt_msg.header = Header() self.jnt_msg.name = [ 'Rev103', 'Rev104', 'Rev105', 'Rev106', 'Rev107', 'Rev108', 'Rev109', 'Rev110', 'Rev111', 'Rev112', 'Rev113', 'Rev114', 'Rev115', 'Rev116', 'Rev117', 'Rev118', 'Rev119', 'Rev120', 'Rev121' ] """self.jnt_msg.name = ['Rev105', 'Rev111', 'Rev117', 'Rev104', 'Rev110', 'Rev116', 'Rev103', 'Rev109', 'Rev115', 'Rev108', 'Rev114', 'Rev120', 'Rev107', 'Rev113', 'Rev119', 'Rev106', 'Rev112', 'Rev118', 'Rev121']""" self.jnt_msg.velocity = [] self.jnt_msg.effort = [] rospy.on_shutdown(self.shutdown_node) rospy.loginfo("Ready for publishing") rate = rospy.Rate(50) while not rospy.is_shutdown(): self.odom_trans.header.stamp = rospy.Time.now() self.odom_trans.transform.translation.x = 0 self.odom_trans.transform.translation.y = 0 self.odom_trans.transform.translation.z = 0 self.odom_trans.transform.rotation = euler_to_quaternion( 0, 0, 0) # roll,pitch,yaw self.odom_broadcaster.sendTransform(self.odom_trans) self.publish_jnt() rate.sleep()
def _handle_odom_tf(self, msg): # Given the pose of the vehicle in waterlinked frame, output the map -> odom tf transformation if not self._odom_updated: point = PointStamped( Header(0, rospy.Time.from_sec(0.0), msg.header.frame_id), msg.pose.pose.position) try: point_map = self._tf_buff.transform(point, self._map_frame_id) except Exception as e: rospy.logerr_throttle( 5, "{} | {}".format(rospy.get_name(), e.message)) return # Odom is always at same depth as map self._map_odom_tf = TransformStamped( Header(0, rospy.Time.now(), self._map_frame_id), self._odom_frame_id, Transform(Vector3(point_map.point.x, point_map.point.y, 0), Quaternion(0, 0, 0, 1))) self._odom_updated = True
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
master_gps = rospy.Publisher('gps_datum', NavSatFix, queue_size=5) master_msg = NavSatFix() master_msg.altitude = alt master_msg.longitude = lon master_msg.latitude = lat master_msg.status = NavSatStatus() master_msg.status.status = master_msg.status.STATUS_FIX master_msg.status.service = master_msg.status.SERVICE_GPS master_msg.position_covariance_type = master_msg.COVARIANCE_TYPE_UNKNOWN master_msg.position_covariance = [-1, 0, 0, 0, 0, 0, 0, 0, 0] pose_pub = rospy.Publisher('waterlinked/pose_with_cov_stamped', PoseWithCovarianceStamped, queue_size=5) buff = Buffer() TransformListener(buff) rospy.Subscriber("mavros/global_position/global", NavSatFix, handle_gps, (pose_pub, buff)) map_to_waterlink = TransformStamped( Header(0, rospy.Time.now(), 'map'), 'waterlinked', Transform( None, Quaternion(*Rotation.from_euler('xyz', [0, 0, 90 - heading_offset], degrees=True).as_quat().tolist()))) StaticTransformBroadcaster().sendTransform(map_to_waterlink) rate = rospy.Rate(4.0) while not rospy.is_shutdown(): master_msg.header.seq += 1 master_msg.header.stamp = rospy.Time.now() master_gps.publish(master_msg) rate.sleep()
def __init__(self): rclpy.init() super().__init__('face_detection') qos_profile = QoSProfile(depth=10) self.broadcaster = TransformBroadcaster(self, qos=qos_profile) self.pub_point = self.create_publisher(PointStamped, "/head_detection", 10) #Image publisher #self.pub_depth_img = self.create_publisher(sensor_msgs.msg.Image, "/depth_img", 10) self.pub_rectified_img = self.create_publisher(Image, "/rectified_img", 10) # Transform declaration self.transform = TransformStamped() self.transform.header.frame_id = 'oak-d_camera_center' self.transform.child_frame_id = 'head' # Detect face syncNN = True self.flipRectified = True # Get argument first model_filename = 'face-detection-retail-0004_openvino_2021.2_4shave.blob' nnPath = os.path.join(get_package_share_directory('blindbuy_oakd'), 'models', model_filename) print(nnPath) # Start defining a pipeline self.pipeline = dai.Pipeline() manip = self.pipeline.createImageManip() manip.initialConfig.setResize(300, 300) # The NN model expects BGR input. By default ImageManip output type would be same as input (gray in this case) manip.initialConfig.setFrameType(dai.RawImgFrame.Type.BGR888p) manip.setKeepAspectRatio( False) #Squeeze image without cropping to fit 300x300 nn input # Define a neural network that will make predictions based on the source frames spatialDetectionNetwork = self.pipeline.createMobileNetSpatialDetectionNetwork( ) spatialDetectionNetwork.setConfidenceThreshold(0.5) spatialDetectionNetwork.setBlobPath(nnPath) spatialDetectionNetwork.input.setBlocking(False) spatialDetectionNetwork.setBoundingBoxScaleFactor(0.5) spatialDetectionNetwork.setDepthLowerThreshold(100) spatialDetectionNetwork.setDepthUpperThreshold(5000) manip.out.link(spatialDetectionNetwork.input) # Create outputs xoutManip = self.pipeline.createXLinkOut() xoutManip.setStreamName("right") if (syncNN): spatialDetectionNetwork.passthrough.link(xoutManip.input) else: manip.out.link(xoutManip.input) depthRoiMap = self.pipeline.createXLinkOut() depthRoiMap.setStreamName("boundingBoxDepthMapping") xoutDepth = self.pipeline.createXLinkOut() xoutDepth.setStreamName("depth") nnOut = self.pipeline.createXLinkOut() nnOut.setStreamName("detections") spatialDetectionNetwork.out.link(nnOut.input) spatialDetectionNetwork.boundingBoxMapping.link(depthRoiMap.input) monoLeft = self.pipeline.createMonoCamera() monoRight = self.pipeline.createMonoCamera() stereo = self.pipeline.createStereoDepth() monoLeft.setResolution( dai.MonoCameraProperties.SensorResolution.THE_400_P) monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT) monoRight.setResolution( dai.MonoCameraProperties.SensorResolution.THE_400_P) monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT) stereo.setOutputDepth(True) stereo.setConfidenceThreshold(255) stereo.setOutputRectified(True) stereo.rectifiedRight.link(manip.inputImage) monoLeft.out.link(stereo.left) monoRight.out.link(stereo.right) stereo.depth.link(spatialDetectionNetwork.inputDepth) spatialDetectionNetwork.passthroughDepth.link(xoutDepth.input) self.detect_face()
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()
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
"Incorrect command line parameters! \"-cam\" cannot be used with \"-vid\"!" ) 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(