From f528414e87621e03d6bcce63a40061284d6fa61e Mon Sep 17 00:00:00 2001
From: uquzq <ventsi@DESKTOP-J41TDPJ>
Date: Tue, 25 Jun 2024 15:06:08 +0200
Subject: [PATCH] Create publishers for the plot and add 2dof 'plotjuggler' tab
---
plotjuggler/2dof_plotjuggler.xml | 71 +++++++++++++++++++
.../forward_kinematics_node_2dof_1.py | 23 +++++-
2 files changed, 92 insertions(+), 2 deletions(-)
create mode 100644 plotjuggler/2dof_plotjuggler.xml
diff --git a/plotjuggler/2dof_plotjuggler.xml b/plotjuggler/2dof_plotjuggler.xml
new file mode 100644
index 0000000..ad71f03
--- /dev/null
+++ b/plotjuggler/2dof_plotjuggler.xml
@@ -0,0 +1,71 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root>
+ <tabbed_widget parent="main_window" name="Main Window">
+ <Tab tab_name="tab1" containers="1">
+ <Container>
+ <DockSplitter count="2" sizes="0.5;0.5" orientation="-">
+ <DockArea name="...">
+ <plot flip_x="false" style="Lines" mode="TimeSeries" flip_y="false">
+ <range bottom="-2.238750" top="91.788750" left="44.599492" right="54.549759"/>
+ <limitY/>
+ <curve color="#1f77b4" name="/angle_2dof/plotjuggler/data"/>
+ </plot>
+ </DockArea>
+ <DockArea name="...">
+ <plot flip_x="false" style="Lines" mode="TimeSeries" flip_y="false">
+ <range bottom="-0.024875" top="1.019875" left="44.599556" right="54.549822"/>
+ <limitY/>
+ <curve color="#d62728" name="/height_2dof/plotjuggler/data"/>
+ </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_2dof/plotjuggler;/height_2dof/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_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
index ee5e6dd..07d8cac 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
@@ -20,6 +20,9 @@ class Forward_Kinematics_Node_2Dof(Node):
# Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.path_publisher_ = self.create_publisher(Path, 'visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
+ # create a publisher for the plotjuggler data
+ self.angle_publisher_ = self.create_publisher(Float64, '/angle_2dof/plotjuggler', 10)
+ self.height_publisher_ = self.create_publisher(Float64, '/height_2dof/plotjuggler', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -33,17 +36,21 @@ class Forward_Kinematics_Node_2Dof(Node):
def transformation(self):
# Define the desired rotation
- theta = -pi / 2
+ theta = pi / 2
# Split theta
theta_fraction = self.phi * theta
+ self.publish_angle(theta_fraction)
+
# Definte the desired height
h = 1
#Split h
h_fraction = self.phi * h
+ self.publish_height(h_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 the matrix
intermediate_rotation = np.array([
@@ -99,6 +106,18 @@ class Forward_Kinematics_Node_2Dof(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 main(args = None):
rclpy.init(args = args)
node = Forward_Kinematics_Node_2Dof()
--
GitLab