Skip to content

Commit 8bc6421

Browse files
committed
Add plotjuggler diagram and fix pre-commit issues
1 parent 6ccf7aa commit 8bc6421

File tree

6 files changed

+51
-67
lines changed

6 files changed

+51
-67
lines changed

example_16/bringup/config/plotjuggler.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@
6868
<plugin ID="Fast Fourier Transform"/>
6969
<plugin ID="Quaternion to RPY"/>
7070
<plugin ID="Reactive Script Editor">
71-
<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;"/>
71+
<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++ indices, not Lua indices&#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;"/>
7272
<scripts/>
7373
</plugin>
7474
<plugin ID="CSV Exporter"/>

example_16/bringup/launch/demo_test.launch.py

Lines changed: 18 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -18,11 +18,23 @@
1818
from launch_ros.substitutions import FindPackageShare
1919
from launch.actions import ExecuteProcess
2020

21+
2122
def generate_launch_description():
2223

23-
return LaunchDescription([
24-
ExecuteProcess(
25-
cmd=['python3', PathJoinSubstitution([FindPackageShare("ros2_control_demo_example_16"), "launch", "demo_test_helper.py"])],
26-
output='screen'
27-
)
28-
])
24+
return LaunchDescription(
25+
[
26+
ExecuteProcess(
27+
cmd=[
28+
"python3",
29+
PathJoinSubstitution(
30+
[
31+
FindPackageShare("ros2_control_demo_example_16"),
32+
"launch",
33+
"demo_test_helper.py",
34+
]
35+
),
36+
],
37+
output="screen",
38+
)
39+
]
40+
)

example_16/bringup/launch/demo_test_helper.py

Lines changed: 22 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -21,33 +21,41 @@
2121

2222
class DiffbotChainedControllersTest(Node):
2323
def __init__(self):
24-
super().__init__('diffbot_chained_controllers_demo_helper_node')
24+
super().__init__("diffbot_chained_controllers_demo_helper_node")
2525
# Enable feedforward control via service call
26-
self.client_left_ = self.create_client(SetBool, '/pid_controller_left_wheel_joint/set_feedforward_control')
27-
self.client_right_ = self.create_client(SetBool, '/pid_controller_right_wheel_joint/set_feedforward_control')
28-
self.publisher_ = self.create_publisher(TwistStamped, '/cmd_vel', 10)
29-
26+
self.client_left_ = self.create_client(
27+
SetBool, "/pid_controller_left_wheel_joint/set_feedforward_control"
28+
)
29+
self.client_right_ = self.create_client(
30+
SetBool, "/pid_controller_right_wheel_joint/set_feedforward_control"
31+
)
32+
self.publisher_ = self.create_publisher(TwistStamped, "/cmd_vel", 10)
33+
3034
def set_feedforward_control(self):
31-
while not self.client_left_.wait_for_service(timeout_sec=1.0):
32-
self.get_logger().info('Waiting for left feedforward control service to be available...')
35+
while not self.client_left_.wait_for_service(timeout_sec=1.0):
36+
self.get_logger().info(
37+
"Waiting for left feedforward control service to be available..."
38+
)
3339
while not self.client_right_.wait_for_service(timeout_sec=1.0):
34-
self.get_logger().info('Waiting for right feedforward control service to be available...')
40+
self.get_logger().info(
41+
"Waiting for right feedforward control service to be available..."
42+
)
3543

3644
request_left = SetBool.Request()
3745
request_left.data = True
3846
future_left = self.client_left_.call_async(request_left)
3947

4048
request_right = SetBool.Request()
4149
request_right.data = True
42-
future_right = self.client_right_ .call_async(request_right)
50+
future_right = self.client_right_.call_async(request_right)
4351

4452
rclpy.spin_until_future_complete(self, future_left)
4553
rclpy.spin_until_future_complete(self, future_right)
4654

47-
self.get_logger().info('Enabled feedforward control for both wheels.')
55+
self.get_logger().info("Enabled feedforward control for both wheels.")
4856

4957
def publish_cmd_vel(self, delay=0.1):
50-
58+
5159
twist_msg = TwistStamped()
5260
twist_msg.twist.linear.x = 0.7
5361
twist_msg.twist.linear.y = 0.0
@@ -62,9 +70,9 @@ def publish_cmd_vel(self, delay=0.1):
6270
time.sleep(delay)
6371

6472

65-
if __name__ == '__main__':
66-
rclpy.init()
67-
test_node = DiffbotChainedControllersTest()
73+
if __name__ == "__main__":
74+
rclpy.init()
75+
test_node = DiffbotChainedControllersTest()
6876
test_node.set_feedforward_control()
6977
test_node.publish_cmd_vel(delay=0.1)
7078
rclpy.spin(test_node)

example_16/bringup/launch/plotjuggler.launch.py

Lines changed: 0 additions & 41 deletions
This file was deleted.
175 KB
Loading

example_16/doc/userdoc.rst

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -187,11 +187,12 @@ Before we proceed, we stop all previous steps from terminal and start from the b
187187
188188
Like before, if you can see an orange box in *RViz*, everything has started properly.
189189

190-
2. To start the plotjuggler with a provided layout file, open another terminal and execute
190+
2. To start the plotjuggler with a provided layout file(plotjuggler.xml), open another terminal and run following command.
191191

192192
.. code-block:: shell
193193
194-
ros2 launch ros2_control_demo_example_16 plotjuggler.launch.py
194+
# replace {PARENT_DIR_OF_LAYOUT_FILE} with actual parent directory of the layout file
195+
ros2 run plotjuggler plotjuggler --layout {PARENT_DIR_OF_LAYOUT_FILE}/plotjuggler.xml
195196
196197
After this, you will see a few dialogs popping up. For example:
197198

@@ -211,9 +212,9 @@ Click 'Yes' for the first dialog and 'OK" to the follow two dialogs, then you wi
211212
212213
4. From the plotjuggler, you can see the controllers' states and commands being plotted, similar to following figure. From the figure, the DiffBot's wheel velocities and commands from PID controllers are converged to the target velocity fairly quickly.
213214

214-
.. image:: /images/example_16/plotjuggler_controller_states.png
215-
:align: center
216-
:alt: Visualization via Plotjuggler
215+
.. image:: diffbot_velocities.png
216+
:width: 400
217+
:alt: Plotjuggler visualization of Diffbot velocities and commands
217218

218219
TODO: add png here.
219220

@@ -234,6 +235,10 @@ Files used for this demo
234235

235236
* Hardware interface plugin: `diffbot_system.cpp <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_16/hardware/diffbot_system.cpp>`__
236237

238+
* Demo helper utility:
239+
240+
+ demo test helper node: `demo_test_helper.py <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_16/bringup/launch/demo_test_helper.py>`__
241+
+ demo test launch file: `demo_test.launch.py <https://github.com/ros-controls/ros2_control_demos/tree/{REPOS_FILE_BRANCH}/example_16/bringup/launch/demo_test.launch.py>`__
237242

238243
Controllers from this demo
239244
--------------------------

0 commit comments

Comments
 (0)