From 76b54ff2d4a8831db52f31159875155cc9de7b00 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Thu, 8 Aug 2024 21:02:20 +0200 Subject: [PATCH] Add Stamped option for Joy --- packages/studio-base/src/panels/Joy/Joy.tsx | 132 +++++++++++------- .../Joy/{DirectionalPad.tsx => JoyVisual.tsx} | 10 +- .../studio-base/src/panels/Joy/styles.css | 30 ++-- 3 files changed, 97 insertions(+), 75 deletions(-) rename packages/studio-base/src/panels/Joy/{DirectionalPad.tsx => JoyVisual.tsx} (96%) diff --git a/packages/studio-base/src/panels/Joy/Joy.tsx b/packages/studio-base/src/panels/Joy/Joy.tsx index 81b852e86a..eb9a9ac964 100644 --- a/packages/studio-base/src/panels/Joy/Joy.tsx +++ b/packages/studio-base/src/panels/Joy/Joy.tsx @@ -6,7 +6,7 @@ import * as _ from "lodash-es"; import { useCallback, useEffect, useLayoutEffect, useState } from "react"; import { DeepPartial } from "ts-essentials"; -import { ros1 } from "@foxglove/rosmsg-msgs-common"; +import { ros2humble } from "@foxglove/rosmsg-msgs-common"; import { PanelExtensionContext, SettingsTreeAction, @@ -17,7 +17,7 @@ import { import EmptyState from "@foxglove/studio-base/components/EmptyState"; import ThemeProvider from "@foxglove/studio-base/theme/ThemeProvider"; -import DirectionalPad from "./DirectionalPad"; +import JoyVisual from "./JoyVisual"; type JoyProps = { context: PanelExtensionContext; @@ -37,6 +37,7 @@ type Axis = { field: string; limit: number }; type Config = { topic: undefined | string; publishRate: number; + stamped: boolean; xAxis: Axis; yAxis: Axis; }; @@ -52,6 +53,11 @@ function buildSettingsTree(config: Config, topics: readonly Topic[]): SettingsTr value: config.topic, items: topics.map((t) => t.name), }, + stamped: { + label: "Stamped", + input: "boolean", + value: config.stamped, + }, }, children: { xAxis: { @@ -105,13 +111,14 @@ function Joy(props: JoyProps): JSX.Element { const [speed, setVelocity] = useState<{ x: number; y: number } | undefined>(); const [topics, setTopics] = useState([]); - // resolve an initial config which may have some missing fields into a full config + // Resolve an initial config which may have some missing fields into a full config const [config, setConfig] = useState(() => { const partialConfig = context.initialState as DeepPartial; const { topic, publishRate = 5, + stamped = false, xAxis: { field: xAxisField = "linear-x", limit: xLimit = 1 } = {}, yAxis: { field: yAxisField = "angular-z", limit: yLimit = 1 } = {}, } = partialConfig; @@ -119,6 +126,7 @@ function Joy(props: JoyProps): JSX.Element { return { topic, publishRate, + stamped, xAxis: { field: xAxisField, limit: xLimit }, yAxis: { field: yAxisField, limit: yLimit }, }; @@ -136,7 +144,7 @@ function Joy(props: JoyProps): JSX.Element { }); }, []); - // setup context render handler and render done handling + // Setup context render handler and render done handling const [renderDone, setRenderDone] = useState<() => void>(() => () => { }); const [colorScheme, setColorScheme] = useState<"dark" | "light">("light"); useLayoutEffect(() => { @@ -161,75 +169,91 @@ function Joy(props: JoyProps): JSX.Element { saveState(config); }, [config, context, saveState, settingsActionHandler, topics]); - // advertise topic - const { topic: currentTopic } = config; + // Advertise topic + const { topic: currentTopic, stamped } = config; useLayoutEffect(() => { if (!currentTopic) { return; } - context.advertise?.(currentTopic, "geometry_msgs/Twist", { - datatypes: new Map([ - ["geometry_msgs/Vector3", ros1["geometry_msgs/Vector3"]], - ["geometry_msgs/Twist", ros1["geometry_msgs/Twist"]], - ]), - }); + const messageType = stamped ? "geometry_msgs/TwistStamped" : "geometry_msgs/Twist"; + const datatypesMap = stamped + ? new Map([ + ["std_msgs/Header", ros2humble["std_msgs/Header"]], + ["geometry_msgs/Vector3", ros2humble["geometry_msgs/Vector3"]], + ["geometry_msgs/Twist", ros2humble["geometry_msgs/Twist"]], + ["geometry_msgs/TwistStamped", ros2humble["geometry_msgs/TwistStamped"]], + ]) + : new Map([ + ["geometry_msgs/Vector3", ros2humble["geometry_msgs/Vector3"]], + ["geometry_msgs/Twist", ros2humble["geometry_msgs/Twist"]], + ]); + + context.advertise?.(currentTopic, messageType, { datatypes: datatypesMap }); return () => { context.unadvertise?.(currentTopic); }; - }, [context, currentTopic]); + }, [context, currentTopic, stamped]); - useLayoutEffect(() => { - if (speed == undefined || !currentTopic) { - return; - } - - const message = { - linear: { x: 0, y: 0, z: 0 }, - angular: { x: 0, y: 0, z: 0 }, + const getRosTimestamp = () => { + const now = Date.now(); + return { + sec: Math.floor(now / 1000), + nanosec: (now % 1000) * 1e6, }; + }; - function setTwistValue(axis: Axis, value: number) { - switch (axis.field) { - case "linear-x": - message.linear.x = value; - break; - case "linear-y": - message.linear.y = value; - break; - case "linear-z": - message.linear.z = value; - break; - case "angular-x": - message.angular.x = value; - break; - case "angular-y": - message.angular.y = value; - break; - case "angular-z": - message.angular.z = value; - break; - } + const createMessage = () => { + if (config.stamped) { + return { + header: { + stamp: getRosTimestamp(), + frame_id: "base_link", + }, + twist: { + linear: { x: 0, y: 0, z: 0 }, + angular: { x: 0, y: 0, z: 0 }, + }, + }; + } else { + return { + linear: { x: 0, y: 0, z: 0 }, + angular: { x: 0, y: 0, z: 0 }, + }; } + }; - setTwistValue(config.xAxis, speed.x); - setTwistValue(config.yAxis, speed.y); + const setTwistValue = (message: any, axis: Axis, value: number) => { + const target = config.stamped ? message.twist : message; + const [category, direction] = axis.field.split("-"); + if (category && direction) { + target[category][direction] = value; + } + }; - // don't publish if rate is 0 or negative - this is a config error on user's part - if (config.publishRate <= 0) { + useLayoutEffect(() => { + if (speed == undefined || !currentTopic) { return; } - const intervalMs = (1000 * 1) / config.publishRate; - context.publish?.(currentTopic, message); - const intervalHandle = setInterval(() => { + const publishMessage = () => { + const message = createMessage(); + setTwistValue(message, config.xAxis, speed.x); + setTwistValue(message, config.yAxis, speed.y); context.publish?.(currentTopic, message); - }, intervalMs); - - return () => { - clearInterval(intervalHandle); }; + + if (config.publishRate > 0) { + const intervalMs = (1000 * 1) / config.publishRate; + publishMessage(); + const intervalHandle = setInterval(publishMessage, intervalMs); + return () => { + clearInterval(intervalHandle); + }; + } else { + return; + } }, [context, config, currentTopic, speed]); useLayoutEffect(() => { @@ -247,7 +271,7 @@ function Joy(props: JoyProps): JSX.Element { Select a publish topic in the panel settings )} {enabled && ( - { setVelocity(value); diff --git a/packages/studio-base/src/panels/Joy/DirectionalPad.tsx b/packages/studio-base/src/panels/Joy/JoyVisual.tsx similarity index 96% rename from packages/studio-base/src/panels/Joy/DirectionalPad.tsx rename to packages/studio-base/src/panels/Joy/JoyVisual.tsx index 83f4083071..305d1886e8 100644 --- a/packages/studio-base/src/panels/Joy/DirectionalPad.tsx +++ b/packages/studio-base/src/panels/Joy/JoyVisual.tsx @@ -7,15 +7,15 @@ import React, { useCallback, useState, useRef } from "react"; import "./styles.css"; // Type for the Joystick Props -type DirectionalPadProps = { +type JoyVisualProps = { disabled?: boolean; onSpeedChange?: (pos: { x: number; y: number }) => void; xLimit?: number; yLimit?: number; }; -// Component for the DirectionalPad -function DirectionalPad(props: DirectionalPadProps): JSX.Element { +// Component for the JoyVisual +function JoyVisual(props: JoyVisualProps): JSX.Element { const { onSpeedChange, disabled = false, xLimit, yLimit } = props; const [speed, setSpeed] = useState<{ x: number; y: number } | undefined>(); const [maxXAxis, setMaxXAxis] = useState(0.5); @@ -141,7 +141,6 @@ function DirectionalPad(props: DirectionalPadProps): JSX.Element {

X Axis

Y Axis