Last active
June 19, 2021 08:06
-
-
Save k-okada/8d4f6169216fd168a4649a3c1d12aa22 to your computer and use it in GitHub Desktop.
plot tf poses
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
cmake_minimum_required(VERSION 3.0.2) | |
project(tf_plot) | |
find_package(catkin REQUIRED) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<launch> | |
<!-- publish world <- earth <- moon transforms --> | |
<node pkg="tf_plot" type="test_tf.py" name="test_tf" /> | |
<!-- 1) publish as PoseStamped --> | |
<node pkg="jsk_topic_tools" type="tf_to_pose.py" name="tf_pose" > | |
<rosparam> | |
src_frame: world | |
dst_frame: moon | |
rate: 1000 | |
</rosparam> | |
</node> | |
<!-- 2) publish as TF via Transform --> | |
<node pkg="jsk_topic_tools" type="tf_to_transform.py" name="tf_transform" > | |
<rosparam> | |
parent_frame_id: world | |
child_frame_id: moon | |
duration: 1.0 | |
rate: 1000 | |
</rosparam> | |
</node> | |
<node pkg="topic_tools" type="transform" name="transform" | |
args="/tf_transform/output /tf tf2_msgs/TFMessage | |
'tf2_msgs.msg.TFMessage([geometry_msgs.msg.TransformStamped(header=m.header, child_frame_id="moon2", transform=m.transform)])' --import tf2_msgs geometry_msgs --wait-for-start"/> | |
<!-- https://answers.ros.org/question/193512/rebroadcast-tf-transforms-with-different-frame_id-on-the-fly/ | |
<node pkg="tf_plot" type="transform_inverter.py" name="transform_inverter" > | |
<rosparam> | |
parent_frame: /world | |
child_frame: /moon | |
new_parent_frame: /moon2 | |
</rosparam> | |
</node> | |
--> | |
<!-- 3) publish as Path --> | |
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server"> | |
<rosparam> | |
target_frame_name: world | |
source_frame_name: moon | |
trajectory_update_rate: 1000 | |
trajectory_publish_rate: 10 | |
</rosparam> | |
</node> | |
<!-- visualize world <- moon --> | |
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find tf_plot)/rviz.rviz" /> | |
<!-- visualize both TF and PoseStamped --> | |
<!-- wait for https://github.com/facontidavide/PlotJuggler/issues/201 | |
<node pkg="plotjuggler" type="plotjuggler" name="plotjuggler" args="-l $(find tf_plot)/plot.xml" /> | |
--> | |
<!-- visualize PoseStamped --> | |
<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot" args="-e /tf_pose/output/pose/position/x /tf_pose/output/pose/position/y" /> | |
</launch> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<?xml version="1.0"?> | |
<package format="2"> | |
<name>tf_plot</name> | |
<version>0.0.0</version> | |
<description>The tf_plot package</description> | |
<maintainer email="kei.okada@gmail.com">k-okada</maintainer> | |
<license>BSD</license> | |
<buildtool_depend>catkin</buildtool_depend> | |
<depend>hector_trajectory_server</depend> | |
<depend>jsk_topic_tools</depend> | |
<depend>jsk_rviz_plugin</depend> | |
<depend>plotjuggler</depend> | |
<depend>rviz</depend> | |
<depend>rqt_plot</depend> | |
<depend>tf</depend> | |
<export> | |
</export> | |
</package> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<?xml version='1.0' encoding='UTF-8'?> | |
<root version="2.3.8"> | |
<tabbed_widget parent="main_window" name="Main Window"> | |
<Tab containers="1" tab_name="tab1"> | |
<Container> | |
<DockSplitter sizes="1" orientation="-" count="1"> | |
<DockArea name="..."> | |
<plot style="Lines" mode="TimeSeries"> | |
<range top="1.000000" right="1.000000" left="0.000000" bottom="0.000000"/> | |
<limitY/> | |
</plot> | |
</DockArea> | |
</DockSplitter> | |
</Container> | |
</Tab> | |
<currentTabIndex index="0"/> | |
</tabbed_widget> | |
<use_relative_time_offset enabled="1"/> | |
<!-- - - - - - - - - - - - - - - --> | |
<!-- - - - - - - - - - - - - - - --> | |
<Plugins> | |
<plugin ID="DataLoad CSV"> | |
<default time_axis=""/> | |
</plugin> | |
<plugin ID="DataLoad ROS bags"> | |
<use_header_stamp value="false"/> | |
<use_renaming_rules value="true"/> | |
<discard_large_arrays value="true"/> | |
<max_array_size value="100"/> | |
</plugin> | |
<plugin ID="DataLoad ULog"/> | |
<plugin ID="LSL Subscriber"/> | |
<plugin ID="MQTT Subscriber"/> | |
<plugin ID="ROS Topic Subscriber"> | |
<use_header_stamp value="true"/> | |
<use_renaming_rules value="true"/> | |
<discard_large_arrays value="true"/> | |
<max_array_size value="100"/> | |
</plugin> | |
<plugin ID="UDP Server"/> | |
<plugin ID="WebSocket Server"/> | |
<plugin ID="ZMQ Subscriber"/> | |
<plugin status="idle" ID="CSV Exporter"/> | |
<plugin status="idle" ID="ROS /rosout Visualization"/> | |
<plugin status="idle" ID="ROS Topic Re-Publisher"/> | |
</Plugins> | |
<!-- - - - - - - - - - - - - - - --> | |
<previouslyLoaded_Datafiles/> | |
<!-- - - - - - - - - - - - - - - --> | |
<customMathEquations/> | |
<snippets/> | |
<!-- - - - - - - - - - - - - - - --> | |
</root> | |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Panels: | |
- Class: rviz/Displays | |
Help Height: 78 | |
Name: Displays | |
Property Tree Widget: | |
Expanded: | |
- /Global Options1 | |
- /Status1 | |
- /TFTrajectory1 | |
- /Path1 | |
Splitter Ratio: 0.5 | |
Tree Height: 549 | |
- Class: rviz/Selection | |
Name: Selection | |
- Class: rviz/Tool Properties | |
Expanded: | |
- /2D Pose Estimate1 | |
- /2D Nav Goal1 | |
- /Publish Point1 | |
Name: Tool Properties | |
Splitter Ratio: 0.5886790156364441 | |
- Class: rviz/Views | |
Expanded: | |
- /Current View1 | |
Name: Views | |
Splitter Ratio: 0.5 | |
- Class: rviz/Time | |
Experimental: false | |
Name: Time | |
SyncMode: 0 | |
SyncSource: "" | |
Preferences: | |
PromptSaveOnExit: true | |
Toolbars: | |
toolButtonStyle: 2 | |
Visualization Manager: | |
Class: "" | |
Displays: | |
- Alpha: 0.5 | |
Cell Size: 1 | |
Class: rviz/Grid | |
Color: 160; 160; 164 | |
Enabled: true | |
Line Style: | |
Line Width: 0.029999999329447746 | |
Value: Lines | |
Name: Grid | |
Normal Cell Count: 0 | |
Offset: | |
X: 0 | |
Y: 0 | |
Z: 0 | |
Plane: XY | |
Plane Cell Count: 10 | |
Reference Frame: <Fixed Frame> | |
Value: true | |
- Class: rviz/TF | |
Enabled: true | |
Frame Timeout: 15 | |
Frames: | |
All Enabled: true | |
earth: | |
Value: true | |
moon: | |
Value: true | |
moon2: | |
Value: true | |
world: | |
Value: true | |
Marker Alpha: 1 | |
Marker Scale: 1 | |
Name: TF | |
Show Arrows: true | |
Show Axes: true | |
Show Names: true | |
Tree: | |
world: | |
earth: | |
moon: | |
{} | |
moon2: | |
{} | |
Update Interval: 0 | |
Value: true | |
- Alpha: 1 | |
Axes Length: 1 | |
Axes Radius: 0.10000000149011612 | |
Class: rviz/Pose | |
Color: 255; 25; 0 | |
Enabled: true | |
Head Length: 0.10000000149011612 | |
Head Radius: 0.10000000149011612 | |
Name: Pose | |
Queue Size: 10 | |
Shaft Length: 0.10000000149011612 | |
Shaft Radius: 0.05000000074505806 | |
Shape: Arrow | |
Topic: /tf_pose/output | |
Unreliable: false | |
Value: true | |
- Class: jsk_rviz_plugin/TFTrajectory | |
Enabled: true | |
Name: TFTrajectory | |
Value: true | |
color: 25; 255; 240 | |
duration: 5 | |
frame: moon | |
line_width: 0.009999999776482582 | |
- Alpha: 1 | |
Buffer Length: 1 | |
Class: rviz/Path | |
Color: 25; 255; 0 | |
Enabled: true | |
Head Diameter: 0.30000001192092896 | |
Head Length: 0.20000000298023224 | |
Length: 0.30000001192092896 | |
Line Style: Lines | |
Line Width: 0.029999999329447746 | |
Name: Path | |
Offset: | |
X: 0 | |
Y: 0 | |
Z: 0.20000000298023224 | |
Pose Color: 255; 85; 255 | |
Pose Style: None | |
Queue Size: 1 | |
Radius: 0.029999999329447746 | |
Shaft Diameter: 0.10000000149011612 | |
Shaft Length: 0.10000000149011612 | |
Topic: /trajectory | |
Unreliable: false | |
Value: true | |
Enabled: true | |
Global Options: | |
Background Color: 48; 48; 48 | |
Default Light: true | |
Fixed Frame: world | |
Frame Rate: 30 | |
Name: root | |
Tools: | |
- Class: rviz/Interact | |
Hide Inactive Objects: true | |
- Class: rviz/MoveCamera | |
- Class: rviz/Select | |
- Class: rviz/FocusCamera | |
- Class: rviz/Measure | |
- Class: rviz/SetInitialPose | |
Theta std deviation: 0.2617993950843811 | |
Topic: /initialpose | |
X std deviation: 0.5 | |
Y std deviation: 0.5 | |
- Class: rviz/SetGoal | |
Topic: /move_base_simple/goal | |
- Class: rviz/PublishPoint | |
Single click: true | |
Topic: /clicked_point | |
Value: true | |
Views: | |
Current: | |
Class: rviz/Orbit | |
Distance: 10 | |
Enable Stereo Rendering: | |
Stereo Eye Separation: 0.05999999865889549 | |
Stereo Focal Distance: 1 | |
Swap Stereo Eyes: false | |
Value: false | |
Field of View: 0.7853981852531433 | |
Focal Point: | |
X: 0 | |
Y: 0 | |
Z: 0 | |
Focal Shape Fixed Size: true | |
Focal Shape Size: 0.05000000074505806 | |
Invert Z Axis: false | |
Name: Current View | |
Near Clip Distance: 0.009999999776482582 | |
Pitch: 0.785398006439209 | |
Target Frame: <Fixed Frame> | |
Yaw: 0.785398006439209 | |
Saved: ~ | |
Window Geometry: | |
Displays: | |
collapsed: false | |
Height: 846 | |
Hide Left Dock: false | |
Hide Right Dock: false | |
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 | |
Selection: | |
collapsed: false | |
Time: | |
collapsed: false | |
Tool Properties: | |
collapsed: false | |
Views: | |
collapsed: false | |
Width: 1200 | |
X: 72 | |
Y: 27 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
import rospy | |
import time | |
import tf | |
import math | |
if __name__ == '__main__': | |
rospy.init_node('test_tf') | |
br = tf.TransformBroadcaster() | |
rate = rospy.Rate(10.0) | |
i = 0 | |
while not rospy.is_shutdown(): | |
now = rospy.Time.now() | |
print(now.secs % 10) | |
if now.secs % 3 != 0: | |
i = i + 0.05 * math.pi | |
br.sendTransform((3.0 * math.sin(i), 3.0 * math.cos(i), 0.0), | |
(0.0, 0.0, 0.0, 1.0), | |
now, | |
"earth", | |
"world") | |
br.sendTransform((math.cos(3.0 * i), math.sin(3.0 * i), 0.0), | |
(0.0, 0.0, 0.0, 1.0), | |
now, | |
"moon", | |
"earth") | |
rate.sleep() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment