From 5699fab7f253273323c5240e24aa49d2a16534e6 Mon Sep 17 00:00:00 2001
From: uquzq <ventsi@DESKTOP-J41TDPJ>
Date: Tue, 25 Jun 2024 21:19:56 +0200
Subject: [PATCH] Create publishers for the 3dof jugglerplot and add 3dof tab
---
plotjuggler/3dof_plotjuggler.xml | 78 +++++++++++++++++++
.../forward_kinematics_node_3dof_2.py | 29 ++++++-
2 files changed, 105 insertions(+), 2 deletions(-)
create mode 100644 plotjuggler/3dof_plotjuggler.xml
diff --git a/plotjuggler/3dof_plotjuggler.xml b/plotjuggler/3dof_plotjuggler.xml
new file mode 100644
index 0000000..95c0dca
--- /dev/null
+++ b/plotjuggler/3dof_plotjuggler.xml
@@ -0,0 +1,78 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget name="Main Window" parent="main_window">
+ <Tab tab_name="tab1" containers="1">
+ <Container>
+ <DockSplitter sizes="0.333333;0.333333;0.333333" orientation="-" count="3">
+ <DockArea name="...">
+ <plot mode="TimeSeries" flip_x="false" style="Lines" flip_y="false">
+ <range right="59.950827" bottom="-2.238750" left="50.000115" top="91.788750"/>
+ <limitY/>
+ <curve name="/angle_3dof/plotjuggler/data" color="#1f77b4"/>
+ </plot>
+ </DockArea>
+ <DockArea name="...">
+ <plot mode="TimeSeries" flip_x="false" style="Lines" flip_y="false">
+ <range right="66.100593" bottom="-0.009150" left="60.000176" top="0.375150"/>
+ <limitY/>
+ <curve name="/height_3dof/plotjuggler/data" color="#d62728"/>
+ </plot>
+ </DockArea>
+ <DockArea name="...">
+ <plot mode="TimeSeries" flip_x="false" style="Lines" flip_y="false">
+ <range right="66.100673" bottom="-0.005337" left="60.000236" top="0.218838"/>
+ <limitY/>
+ <curve name="/length_3dof/plotjuggler/data" color="#1ac938"/>
+ </plot>
+ </DockArea>
+ </DockSplitter>
+ </Container>
+ </Tab>
+ <currentTabIndex index="0"/>
+ </tabbed_widget>
+ <use_relative_time_offset enabled="1"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <!-- - - - - - - - - - - - - - - -->
+ <Plugins>
+ <plugin ID="DataLoad CSV">
+ <parameters time_axis="" delimiter="0"/>
+ </plugin>
+ <plugin ID="DataLoad MCAP"/>
+ <plugin ID="DataLoad ROS2 bags">
+ <use_header_stamp value="false"/>
+ <discard_large_arrays value="true"/>
+ <max_array_size value="100"/>
+ <boolean_strings_to_number value="true"/>
+ <remove_suffix_from_strings value="true"/>
+ <selected_topics value=""/>
+ </plugin>
+ <plugin ID="DataLoad ULog"/>
+ <plugin ID="ROS2 Topic Subscriber">
+ <use_header_stamp value="false"/>
+ <discard_large_arrays value="true"/>
+ <max_array_size value="100"/>
+ <boolean_strings_to_number value="true"/>
+ <remove_suffix_from_strings value="true"/>
+ <selected_topics value="/angle_3dof/plotjuggler;/height_3dof/plotjuggler;/length_3dof/plotjuggler"/>
+ </plugin>
+ <plugin ID="UDP Server"/>
+ <plugin ID="WebSocket Server"/>
+ <plugin ID="ZMQ Subscriber"/>
+ <plugin ID="Fast Fourier Transform"/>
+ <plugin ID="Quaternion to RPY"/>
+ <plugin ID="Reactive Script Editor">
+ <library code="--[[ Helper function to create a series from arrays

 new_series: a series previously created with ScatterXY.new(name)
 prefix: prefix of the timeseries, before the index of the array
 suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.
 suffix_Y: suffix to complete the name of the series containing the Y value
 timestamp: usually the tracker_time variable
 
 Example:
 
 Assuming we have multiple series in the form:
 
 /trajectory/node.{X}/position/x
 /trajectory/node.{X}/position/y
 
 where {N} is the index of the array (integer). We can create a reactive series from the array with:
 
 new_series = ScatterXY.new("my_trajectory") 
 CreateSeriesFromArray( new_series, "/trajectory/node", "position/x", "position/y", tracker_time );
--]]

function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )
 
 --- clear previous values
 new_series:clear()
 
 --- Append points to new_series
 index = 0
 while(true) do

 x = index;
 -- if not nil, get the X coordinate from a series
 if suffix_X ~= nil then 
 series_x = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_X) )
 if series_x == nil then break end
 x = series_x:atTime(timestamp)	 
 end
 
 series_y = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_Y) )
 if series_y == nil then break end 
 y = series_y:atTime(timestamp)
 
 new_series:push_back(x,y)
 index = index+1
 end
end

--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]

function GetSeriesNamesByPrefix(prefix)
 -- GetSeriesNames(9 is a built-in function
 all_names = GetSeriesNames()
 filtered_names = {}
 for i, name in ipairs(all_names) do
 -- check the prefix
 if name:find(prefix, 1, #prefix) then
 table.insert(filtered_names, name);
 end
 end
 return filtered_names
end

--[[ Modify an existing series, applying offsets to all their X and Y values

 series: an existing timeseries, obtained with TimeseriesView.find(name)
 delta_x: offset to apply to each x value
 delta_y: offset to apply to each y value 
 
--]]

function ApplyOffsetInPlace(series, delta_x, delta_y)
 -- use C++ indeces, not Lua indeces
 for index=0, series:size()-1 do
 x,y = series:at(index)
 series:set(index, x + delta_x, y + delta_y)
 end
end
"/>
+ <scripts/>
+ </plugin>
+ <plugin ID="CSV Exporter"/>
+ <plugin ID="ROS2 Topic Re-Publisher"/>
+ </Plugins>
+ <!-- - - - - - - - - - - - - - - -->
+ <previouslyLoaded_Datafiles/>
+ <previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
+ <!-- - - - - - - - - - - - - - - -->
+ <customMathEquations/>
+ <snippets/>
+ <!-- - - - - - - - - - - - - - - -->
+</root>
+
diff --git a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
index 3ae5796..49f0302 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
+++ b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
@@ -22,6 +22,9 @@ class Forward_Kinematics_Node_3Dof(Node):
self.path_publisher_ = self.create_publisher(Path, 'visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
self.index = 0
+ self.angle_publisher_ = self.create_publisher(Float64, '/angle_3dof/plotjuggler',10)
+ self.height_publisher_ = self.create_publisher(Float64, '/height_3dof/plotjuggler', 10)
+ self.length_publisher_ = self.create_publisher(Float64, '/length_3dof/plotjuggler', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -45,10 +48,12 @@ class Forward_Kinematics_Node_3Dof(Node):
def rotation(self):
# define the desired rotation
- theta = -pi / 2
+ theta = pi / 2
# split theta
theta_fraction = self.phi * theta
+ self.publish_angle(theta_fraction)
+
# define the matrix
intermediate_rotation = np.array([
[cos(theta_fraction), -sin(theta_fraction), 0, 0],
@@ -69,13 +74,17 @@ class Forward_Kinematics_Node_3Dof(Node):
# split h
h_fraction = self.phi * h
+ self.publish_height(h_fraction)
+
# define the desired length
d = 0.35
# split d
d_fraction = self.phi * d
+ self.publish_length(d_fraction)
+
# Define the vector
- start_vector = np.array([-1.0, 0.0, 0.0, 1.0])
+ start_vector = np.array([1.0, 0.0, 0.0, 1.0])
# define translation matrix
intermediate_translation = np.array([
@@ -142,6 +151,22 @@ class Forward_Kinematics_Node_3Dof(Node):
# Publish the marker
self.point_publisher_.publish(marker)
+ def publish_angle(self, angle):
+ angle_msg = Float64()
+ angle_msg.data = angle * 180/ pi
+ self.angle_publisher_.publish(angle_msg)
+ # self.get_logger().info('Publishing: %f' %angle_radians)
+
+ def publish_height(self, height):
+ height_msg = Float64()
+ height_msg.data = height
+ self.height_publisher_.publish(height_msg)
+
+ def publish_length(self, length):
+ length_msg = Float64()
+ length_msg.data = length
+ self.length_publisher_.publish(length_msg)
+
def main(args = None):
rclpy.init(args = args)
--
GitLab