Skip to content
Snippets Groups Projects
Commit 5699fab7 authored by uquzq's avatar uquzq
Browse files

Create publishers for the 3dof jugglerplot and add 3dof tab

parent f528414e
No related branches found
No related tags found
No related merge requests found
<?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&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix: prefix of the timeseries, before the index of the array&#xa; suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y: suffix to complete the name of the series containing the Y value&#xa; timestamp: usually the tracker_time variable&#xa; &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa; /trajectory/node.{X}/position/x&#xa; /trajectory/node.{X}/position/y&#xa; &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa; new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa; CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa; &#xa; --- clear previous values&#xa; new_series:clear()&#xa; &#xa; --- Append points to new_series&#xa; index = 0&#xa; while(true) do&#xa;&#xa; x = index;&#xa; -- if not nil, get the X coordinate from a series&#xa; if suffix_X ~= nil then &#xa; series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa; if series_x == nil then break end&#xa; x = series_x:atTime(timestamp)&#x9; &#xa; end&#xa; &#xa; series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa; if series_y == nil then break end &#xa; y = series_y:atTime(timestamp)&#xa; &#xa; new_series:push_back(x,y)&#xa; index = index+1&#xa; end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa; -- GetSeriesNames(9 is a built-in function&#xa; all_names = GetSeriesNames()&#xa; filtered_names = {}&#xa; for i, name in ipairs(all_names) do&#xa; -- check the prefix&#xa; if name:find(prefix, 1, #prefix) then&#xa; table.insert(filtered_names, name);&#xa; end&#xa; end&#xa; return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa; &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa; -- use C++ indeces, not Lua indeces&#xa; for index=0, series:size()-1 do&#xa; x,y = series:at(index)&#xa; series:set(index, x + delta_x, y + delta_y)&#xa; end&#xa;end&#xa;"/>
<scripts/>
</plugin>
<plugin ID="CSV Exporter"/>
<plugin ID="ROS2 Topic Re-Publisher"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
<previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
<!-- - - - - - - - - - - - - - - -->
<customMathEquations/>
<snippets/>
<!-- - - - - - - - - - - - - - - -->
</root>
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment