diff --git a/src/core/Ros.ts b/src/core/Ros.ts index c4ab8ed82..65b63b853 100644 --- a/src/core/Ros.ts +++ b/src/core/Ros.ts @@ -19,7 +19,7 @@ import { import Topic from "./Topic.js"; import Service from "./Service.js"; import Param from "./Param.js"; -import TFClient from "../tf/TFClient.js"; +import TFClient from "../tf/TFClient"; import ActionClient from "../actionlib/ActionClient.js"; import SimpleActionServer from "../actionlib/SimpleActionServer.js"; import { EventEmitter } from "eventemitter3"; diff --git a/src/tf/BaseTFClient.js b/src/tf/BaseTFClient.ts similarity index 61% rename from src/tf/BaseTFClient.js rename to src/tf/BaseTFClient.ts index 9769da2af..5c0698e36 100644 --- a/src/tf/BaseTFClient.js +++ b/src/tf/BaseTFClient.ts @@ -1,35 +1,41 @@ import Transform from "../math/Transform.js"; import { EventEmitter } from "eventemitter3"; +import Ros from "../core/Ros.js"; +import { tf2_msgs } from "../types/tf2_msgs.js"; /** * Base class for TF Clients that provides common functionality. */ export default class BaseTFClient extends EventEmitter { - frameInfos = {}; + frameInfos: Record< + string, + { transform?: Transform; cbs: ((tf: Transform) => void)[] } + > = {}; republisherUpdateRequested = false; - /** @type {((tf: any) => any) | undefined} */ - _subscribeCB = undefined; _isDisposed = false; - ros; - fixedFrame; - angularThres; - transThres; - rate; - updateDelay; - topicTimeout; - serverName; + ros: Ros; + fixedFrame: string; + angularThres: number; + transThres: number; + rate: number; + updateDelay: number; + topicTimeout: { + sec: number; + nsec: number; + }; + serverName: string; /** - * @param {Object} options - * @param {import('../core/Ros.js').default} options.ros - The ROSLIB.Ros connection handle. - * @param {string} [options.fixedFrame=base_link] - The fixed frame. - * @param {number} [options.angularThres=2.0] - The angular threshold for the TF republisher. - * @param {number} [options.transThres=0.01] - The translation threshold for the TF republisher. - * @param {number} [options.rate=10.0] - The rate for the TF republisher. - * @param {number} [options.updateDelay=50] - The time (in ms) to wait after a new subscription + * @param options + * @param options.ros - The ROSLIB.Ros connection handle. + * @param [options.fixedFrame=base_link] - The fixed frame. + * @param [options.angularThres=2.0] - The angular threshold for the TF republisher. + * @param [options.transThres=0.01] - The translation threshold for the TF republisher. + * @param [options.rate=10.0] - The rate for the TF republisher. + * @param [options.updateDelay=50] - The time (in ms) to wait after a new subscription * to update the TF republisher's list of TFs. - * @param {number} [options.topicTimeout=2.0] - The timeout parameter for the TF republisher. - * @param {string} [options.serverName="/tf2_web_republisher"] - The name of the tf2_web_republisher server. + * @param [options.topicTimeout=2.0] - The timeout parameter for the TF republisher. + * @param [options.serverName="/tf2_web_republisher"] - The name of the tf2_web_republisher server. */ constructor({ ros, @@ -40,6 +46,15 @@ export default class BaseTFClient extends EventEmitter { updateDelay = 50, topicTimeout = 2.0, serverName = "/tf2_web_republisher", + }: { + ros: Ros; + fixedFrame?: string; + angularThres?: number; + transThres?: number; + rate?: number; + updateDelay?: number; + topicTimeout?: number; + serverName?: string; }) { super(); @@ -53,8 +68,8 @@ export default class BaseTFClient extends EventEmitter { const secs = Math.floor(seconds); const nsecs = Math.floor((seconds - secs) * 1000000000); this.topicTimeout = { - secs: secs, - nsecs: nsecs, + sec: secs, + nsec: nsecs, }; this.serverName = serverName; } @@ -63,9 +78,9 @@ export default class BaseTFClient extends EventEmitter { * Process the incoming TF message and send them out using the callback * functions. * - * @param {Object} tf - The TF message from the server. + * @param tf - The TF message from the server. */ - processTFArray(tf) { + processTFArray(tf: tf2_msgs.TFMessage) { tf.transforms.forEach((transform) => { let frameID = transform.child_frame_id; if (frameID[0] === "/") { @@ -73,13 +88,12 @@ export default class BaseTFClient extends EventEmitter { } const info = this.frameInfos[frameID]; if (info) { - info.transform = new Transform({ + const tf = new Transform({ translation: transform.transform.translation, rotation: transform.transform.rotation, }); - info.cbs.forEach((cb) => { - cb(info.transform); - }); + info.transform = tf; + info.cbs.forEach((cb) => cb(tf)); } }, this); } @@ -93,17 +107,13 @@ export default class BaseTFClient extends EventEmitter { throw new Error("updateGoal() must be implemented by subclass"); } - /** - * @callback subscribeCallback - * @param {Transform} callback.transform - The transform data. - */ /** * Subscribe to the given TF frame. * - * @param {string} frameID - The TF frame to subscribe to. - * @param {subscribeCallback} callback - Function with the following params: + * @param frameID - The TF frame to subscribe to. + * @param callback - Function with the following params: */ - subscribe(frameID, callback) { + subscribe(frameID: string, callback: (transform: Transform) => void) { // remove leading slash, if it's there if (frameID.startsWith("/")) { frameID = frameID.substring(1); @@ -120,8 +130,9 @@ export default class BaseTFClient extends EventEmitter { } // if we already have a transform, callback immediately - else if (this.frameInfos[frameID].transform) { - callback(this.frameInfos[frameID].transform); + const transform = this.frameInfos[frameID].transform; + if (transform) { + callback(transform); } this.frameInfos[frameID].cbs.push(callback); } @@ -129,10 +140,10 @@ export default class BaseTFClient extends EventEmitter { /** * Unsubscribe from the given TF frame. * - * @param {string} frameID - The TF frame to unsubscribe from. - * @param {function} callback - The callback function to remove. + * @param frameID - The TF frame to unsubscribe from. + * @param callback - The callback function to remove. */ - unsubscribe(frameID, callback) { + unsubscribe(frameID: string, callback: (transform: Transform) => void) { // remove leading slash, if it's there if (frameID.startsWith("/")) { frameID = frameID.substring(1); diff --git a/src/tf/ROS2TFClient.js b/src/tf/ROS2TFClient.ts similarity index 76% rename from src/tf/ROS2TFClient.js rename to src/tf/ROS2TFClient.ts index b54b298cd..4e8764b69 100644 --- a/src/tf/ROS2TFClient.js +++ b/src/tf/ROS2TFClient.ts @@ -1,3 +1,4 @@ +import { tf2_web_republisher } from "../types/tf2_web_republisher.js"; import Action from "../core/Action.js"; import BaseTFClient from "./BaseTFClient.js"; @@ -5,12 +6,15 @@ import BaseTFClient from "./BaseTFClient.js"; * A TF Client that listens to TFs from tf2_web_republisher using ROS2 actions. */ export default class ROS2TFClient extends BaseTFClient { - goal_id; - actionClient; - currentGoal; + goal_id: string; + actionClient: Action< + tf2_web_republisher.TFSubscriptionGoal, + tf2_web_republisher.TFSubscriptionResult, + tf2_web_republisher.TFSubscriptionFeedback + >; + currentGoal: tf2_web_republisher.TFSubscriptionGoal; - /** @param {ConstructorParameters[0]} options */ - constructor(options) { + constructor(options: ConstructorParameters[0]) { super(options); this.goal_id = ""; @@ -44,7 +48,7 @@ export default class ROS2TFClient extends BaseTFClient { const id = this.actionClient.sendGoal( goalMessage, () => {}, - (feedback) => { + (feedback: tf2_web_republisher.TFSubscriptionFeedback) => { this.processTFArray(feedback); }, ); diff --git a/src/tf/TFClient.js b/src/tf/TFClient.ts similarity index 60% rename from src/tf/TFClient.js rename to src/tf/TFClient.ts index 1aac3a99d..7210baf79 100644 --- a/src/tf/TFClient.js +++ b/src/tf/TFClient.ts @@ -8,6 +8,9 @@ import Goal from "../actionlib/Goal.js"; import Service from "../core/Service.js"; import Topic from "../core/Topic.js"; +import Ros from "../core/Ros.js"; +import { tf2_msgs } from "../types/tf2_msgs.js"; +import { tf2_web_republisher } from "../types/tf2_web_republisher.js"; import BaseTFClient from "./BaseTFClient.js"; @@ -15,28 +18,43 @@ import BaseTFClient from "./BaseTFClient.js"; * A TF Client that listens to TFs from tf2_web_republisher. */ export default class TFClient extends BaseTFClient { - /** @type {Goal|false} */ - currentGoal = false; - /** @type {Topic|false} */ - currentTopic = false; - repubServiceName; - actionClient; - serviceClient; + currentGoal: Goal | false = false; + currentTopic: Topic | false = false; + repubServiceName: string; + actionClient: ActionClient; + serviceClient: Service< + tf2_web_republisher.RepublishTFsRequest, + tf2_web_republisher.RepublishTFsResponse + >; + _subscribeCB: ((tf: tf2_msgs.TFMessage) => void) | undefined = undefined; /** - * @param {Object} options - * @param {import('../core/Ros.js').default} options.ros - The ROSLIB.Ros connection handle. - * @param {string} [options.fixedFrame=base_link] - The fixed frame. - * @param {number} [options.angularThres=2.0] - The angular threshold for the TF republisher. - * @param {number} [options.transThres=0.01] - The translation threshold for the TF republisher. - * @param {number} [options.rate=10.0] - The rate for the TF republisher. - * @param {number} [options.updateDelay=50] - The time (in ms) to wait after a new subscription + * @param options + * @param options.ros - The ROSLIB.Ros connection handle. + * @param [options.fixedFrame=base_link] - The fixed frame. + * @param [options.angularThres=2.0] - The angular threshold for the TF republisher. + * @param [options.transThres=0.01] - The translation threshold for the TF republisher. + * @param [options.rate=10.0] - The rate for the TF republisher. + * @param [options.updateDelay=50] - The time (in ms) to wait after a new subscription * to update the TF republisher's list of TFs. - * @param {number} [options.topicTimeout=2.0] - The timeout parameter for the TF republisher. - * @param {string} [options.serverName="/tf2_web_republisher"] - The name of the tf2_web_republisher server. - * @param {string} [options.repubServiceName="/republish_tfs"] - The name of the republish_tfs service (non groovy compatibility mode only). + * @param [options.topicTimeout=2.0] - The timeout parameter for the TF republisher. + * @param [options.serverName="/tf2_web_republisher"] - The name of the tf2_web_republisher server. + * @param [options.repubServiceName="/republish_tfs"] - The name of the republish_tfs service (non groovy compatibility mode only). */ - constructor({ repubServiceName = "/republish_tfs", ...options }) { + constructor({ + repubServiceName = "/republish_tfs", + ...options + }: { + ros: Ros; + fixedFrame?: string; + angularThres?: number; + transThres?: number; + rate?: number; + updateDelay?: number; + topicTimeout?: number; + serverName?: string; + repubServiceName?: string; + }) { super(options); this.repubServiceName = repubServiceName; @@ -63,7 +81,7 @@ export default class TFClient extends BaseTFClient { * based on the current list of TFs. */ updateGoal() { - const goalMessage = { + const goalMessage: tf2_web_republisher.TFSubscriptionGoal = { source_frames: Object.keys(this.frameInfos), target_frame: this.fixedFrame, angular_thres: this.angularThres, @@ -79,7 +97,7 @@ export default class TFClient extends BaseTFClient { if (this.currentGoal) { this.currentGoal.cancel(); } - this.currentGoal = new Goal({ + this.currentGoal = new Goal({ actionClient: this.actionClient, goalMessage: goalMessage, }); @@ -92,9 +110,8 @@ export default class TFClient extends BaseTFClient { * The service interface has the same parameters as the action, * plus the timeout */ - goalMessage.timeout = this.topicTimeout; this.serviceClient.callService( - goalMessage, + { ...goalMessage, timeout: this.topicTimeout }, this.processResponse.bind(this), ); } @@ -106,9 +123,9 @@ export default class TFClient extends BaseTFClient { * Process the service response and subscribe to the tf republisher * topic. * - * @param {Object} response - The service response containing the topic name. + * @param response - The service response containing the topic name. */ - processResponse(response) { + processResponse(response: tf2_web_republisher.RepublishTFsResponse) { /* * Do not setup a topic subscription if already disposed. Prevents a race condition where * The dispose() function is called before the service call receives a response. @@ -125,7 +142,7 @@ export default class TFClient extends BaseTFClient { this.currentTopic.unsubscribe(this._subscribeCB); } - this.currentTopic = new Topic({ + this.currentTopic = new Topic({ ros: this.ros, name: response.topic_name, messageType: "tf2_web_republisher/TFArray", diff --git a/src/tf/index.ts b/src/tf/index.ts index c87804823..62dd1dc2c 100644 --- a/src/tf/index.ts +++ b/src/tf/index.ts @@ -1,2 +1,2 @@ -export { default as TFClient } from "./TFClient.js"; -export { default as ROS2TFClient } from "./ROS2TFClient.js"; +export { default as TFClient } from "./TFClient"; +export { default as ROS2TFClient } from "./ROS2TFClient"; diff --git a/src/types/geometry_msgs.ts b/src/types/geometry_msgs.ts new file mode 100644 index 000000000..882be5df0 --- /dev/null +++ b/src/types/geometry_msgs.ts @@ -0,0 +1,22 @@ +export namespace geometry_msgs { + export interface TransformStamped { + // TODO: some sort of unholy union type of ros1 header and ros2 header??? + child_frame_id: string; + transform: Transform; + } + export interface Transform { + translation: Vector3; + rotation: Quaternion; + } + export interface Vector3 { + x: number; + y: number; + z: number; + } + export interface Quaternion { + x: number; + y: number; + z: number; + w: number; + } +} diff --git a/src/types/tf2_msgs.ts b/src/types/tf2_msgs.ts new file mode 100644 index 000000000..428cdc8c8 --- /dev/null +++ b/src/types/tf2_msgs.ts @@ -0,0 +1,7 @@ +import { geometry_msgs } from "./geometry_msgs"; + +export namespace tf2_msgs { + export interface TFMessage { + transforms: geometry_msgs.TransformStamped[]; + } +} diff --git a/src/types/tf2_web_republisher.ts b/src/types/tf2_web_republisher.ts new file mode 100644 index 000000000..cc28481ea --- /dev/null +++ b/src/types/tf2_web_republisher.ts @@ -0,0 +1,28 @@ +import { geometry_msgs } from "./geometry_msgs"; + +export namespace tf2_web_republisher { + export interface RepublishTFsRequest extends TFSubscriptionGoal { + timeout: { + sec: number; + nsec: number; + }; + } + + export interface RepublishTFsResponse { + topic_name: string; + } + + export interface TFSubscriptionGoal { + source_frames: string[]; + target_frame: string; + angular_thres: number; + trans_thres: number; + rate: number; + } + + export type TFSubscriptionResult = Record; + + export interface TFSubscriptionFeedback { + transforms: geometry_msgs.TransformStamped[]; + } +}