Merge pull request #56 from KIT-ISAS/feature/evaluation

Feature/evaluation

Author: fennel-labs

README.md

@@ -377,7 +377,17 @@ Both logging styles are fully compatible with *rosconsole*, including *rqt_logge
 
 To monitor the performance of the deployed controllers, it is vital to measure the calculation time as well as the jitter of the `updateHook()`-invocations. For this reason, the *rt_runner* publishes a timestamp of the update invocation time as well as the calculation time on the topic `/rt_runner/iteration_info` after the completion of every iteration.
 
-Additionally, some timing statistics are printed to the console each time the control loop is stopped.
+Additionally, some timing statistics are printed to the console each time the control loop is stopped. If a more detailed analysis is desired, a rosbag can be recorded using
+
+```bash
+rosbag record /rt_runner/iteration_info -o bagname --duration 3h
+```
+
+or a similar command. The resulting rosbag can then be analyzed conveniently with an in-built script that plots the latencies as well as the calculation durations. The necessary command for this is:
+
+```bash
+rosrun rtcf evaluate_performance_rosbag path_to_rosbag frequency
+```
 
 #### Miscellaneous
 

rtcf/scripts/evaluate_performance_rosbag

@@ -0,0 +1,102 @@
+#!/usr/bin/python
+
+import rosbag
+from rospy import Time
+import matplotlib.pyplot as plt
+import numpy as np
+import sys
+import os.path
+
+
+def printHelp():
+    print("Evaluate a rosbag with performance data for the RTCF. The output file is located at the same path as the input file")
+    print("")
+    print("Syntax: rosrun rtcf evaluate_performance_rosbag path_to_rosbag frequency")
+    print("")
+    print("path_to_rosbag   Path to the rosbag-file.")
+    print("frequency        The frequency of the control-loop in the rosbag-file.")
+    print("")
+    print("To record such a rosbag use the following command as a starting point:")
+    print("rosbag record /rt_runner/iteration_info -o bagname --duration 3h")
+    quit()
+
+
+# argument parsing
+if len(sys.argv) != 2+1:
+    printHelp()
+try:
+    frequency = float(sys.argv[2])
+except ValueError:
+    printHelp()
+path = sys.argv[1]
+if not os.path.exists(path):
+    printHelp()
+folder, file = os.path.split(path)
+folder = folder + '/'
+
+
+durations = []
+intervalls = []
+last_time = None
+
+# open rosbag
+print("Opening rosbag. This may take a while...")
+bag = rosbag.Bag(folder+file)
+topics = ['/rt_runner/iteration_info']
+count = bag.get_message_count(topic_filters=topics)
+
+
+print("Reading messages. This may take a while...")
+i = 0
+for topic, msg, t in bag.read_messages(topics=topics):
+    time = msg.stamp.to_nsec()
+    i = i+1
+    if i % int(frequency*60*10) == 0:
+        print("10 minutes processed...")
+    if last_time:
+        durations.append(msg.duration_ns)
+        intervalls.append(time-last_time)
+    last_time = time
+
+bag.close()
+
+durations = np.array(durations)
+intervalls = np.array(intervalls)
+
+durations = durations / 1000.0
+intervalls = intervalls / 1000.0
+
+last_index = durations.size
+
+# get min, max and 99.9 % percentile
+duration_min = np.min(durations)
+duration_max = np.max(durations)
+duration_percentile = np.percentile(durations, 99.9)
+
+intervall_min = np.min(intervalls)
+intervall_max = np.max(intervalls)
+intervall_percentile = np.percentile(intervalls, 99.9)
+
+t = np.arange(0, last_index, 1)
+t = t / (float(frequency)) / 60
+
+print("Found {} samples.".format(t.size))
+print("Plotting the results. This may take a while...")
+
+# TIME SERIES
+fig, axs = plt.subplots(2)
+axs[0].scatter(t, durations, marker='+')
+axs[0].set_ylim(0, 50)
+axs[0].set_xlabel("t in min")
+axs[0].set_ylabel("calculation duration in us")
+
+axs[1].scatter(t, intervalls, marker='+')
+center = 1.0/frequency * 1e6
+axs[1].set_ylim(center - 100, center + 100)
+axs[1].set_xlabel("t in min")
+axs[1].set_ylabel("calculation intervall in us")
+
+output_filename = folder+file+".evaluation.png"
+plt.savefig(output_filename, dpi=600)
+
+print("Plot was written to: {}".format(folder+file+".evaluation.png"))

rtcf_examples/CMakeLists.txt

@@ -32,6 +32,7 @@ add_subdirectory(src/identity)
 add_subdirectory(src/mimo)
 add_subdirectory(src/parameter_handling)
 add_subdirectory(src/logging)
+add_subdirectory(src/paper_example)
 
 # finalize the OROCOS package
 orocos_generate_package()

rtcf_examples/launch/paper_evaluation.launch

@@ -0,0 +1,50 @@
+<launch>
+    <node name="rt_runner" pkg="rtcf" type="rt_runner" output="screen">
+        <rosparam param="mode">"wait_for_components"</rosparam>
+        <rosparam param="ros_mapping_whitelist">".*R3_G2H1|.*R1_H1R2|.*G2_R2"</rosparam>
+        <rosparam param="num_components_expected">5</rosparam>
+        <rosparam param="frequency">2000.0</rosparam>
+        <rosparam param="lock_memory">True</rosparam>
+        <rosparam param="safe_heap_size_kB">4096</rosparam>
+        <rosparam param="safe_stack_size_kB">512</rosparam>
+        <rosparam param="cpu_affinity_mask">02</rosparam>
+    </node>
+
+    <node name="H1" pkg="rtcf" type="rt_launcher" args="rtcf_examples Mimo">
+        <remap from="out1" to="/H1_F1G1"/>
+        <remap from="out2" to="/H1_R1R2"/>
+        <remap from="in1" to="/R3_G2H1"/>
+        <remap from="in2" to="/R1_H1R2"/>
+        <rosparam param="is_first">True</rosparam>
+    </node>
+
+    <node name="F1" pkg="rtcf" type="rt_launcher" args="rtcf_examples Mimo">
+        <remap from="out1" to="/F1_R2R3"/>
+        <remap from="out2" to="/F1_out2"/>
+        <remap from="in1" to="/H1_F1G1"/>
+    </node>
+
+    <node name="R1" pkg="rtcf" type="rt_launcher" args="rtcf_examples Mimo">
+        <remap from="out1" to="/R1_H1R2"/>
+        <remap from="out2" to="/R1_out2"/>
+        <remap from="in1" to="/H1_R1R2"/>
+    </node>
+
+    <node name="R2" pkg="rtcf" type="rt_launcher" args="rtcf_examples Mimo">
+        <remap from="out1" to="/R2_R3"/>
+        <remap from="out2" to="/R2_out2"/>
+        <remap from="in1" to="/F1_R2R3"/>
+        <remap from="in2" to="/H1_R1R2"/>
+        <remap from="in3" to="/R1_H1R2"/>
+        <remap from="in4" to="/G2_R2"/>
+    </node>
+
+    <node name="R3" pkg="rtcf" type="rt_launcher" args="rtcf_examples Mimo">
+        <remap from="out1" to="/R3_G2H1"/>
+        <remap from="out2" to="/R3_out2"/>
+        <remap from="in1" to="/F1_R2R3"/>
+        <remap from="in2" to="/R2_R3"/>
+    </node>
+
+</launch>
+

rtcf_examples/launch/paper_example.launch

@@ -0,0 +1,18 @@
+<launch>
+    <node name="rt_runner" pkg="rtcf" type="rt_runner" output="screen">
+        <rosparam param="mode">"wait_for_components"</rosparam>
+        <rosparam param="ros_mapping_whitelist">".*ros.*"</rosparam>
+        <rosparam param="num_components_expected">2</rosparam>
+        <rosparam param="frequency">1000.0</rosparam>
+    </node>
+    <node name="example1" pkg="rtcf" type="rt_launcher" args="rtcf_examples PaperExample">
+        <remap from="out_port" to="/tmp"/>
+        <remap from="in_port" to="/ros/in"/>
+        <rosparam param="is_first">True</rosparam>
+    </node>
+    <node name="example2" pkg="rtcf" type="rt_launcher" args="rtcf_examples PaperExample">
+        <remap from="out_port" to="/ros/out"/>
+        <remap from="in_port" to="/tmp"/>
+    </node>
+</launch>
+

rtcf_examples/src/mimo/component.cpp

@@ -37,7 +37,25 @@ bool Mimo::startHook() {
     return true;
 }
 
-void Mimo::updateHook() {}
+void Mimo::updateHook() {
+    // read will copy old data, which is fine
+    std_msgs::Float64 in_msg_1_, in_msg_2_, in_msg_3_, in_msg_4_;
+    in_msg_1_.data = 1.0;
+    port_in1_.read(in_msg_1_, false);
+    port_in2_.read(in_msg_2_, false);
+    port_in3_.read(in_msg_3_, false);
+    port_in4_.read(in_msg_4_, false);
+
+    std_msgs::Float64 out_msg_;
+    // just do a plain simple addition with some overflow checking
+    out_msg_.data = in_msg_1_.data + in_msg_2_.data + in_msg_3_.data + in_msg_4_.data;
+    // make sure float does not overflow (pure addition will cause an overflow)
+    if (out_msg_.data > 1e20) {
+        out_msg_.data = 1;
+    }
+    port_out1_.write(out_msg_);
+    port_out2_.write(out_msg_);
+}
 
 void Mimo::stopHook() { NON_RT_INFO("Mimo executes stopping !"); }
 

rtcf_examples/src/paper_example/CMakeLists.txt

@@ -0,0 +1,2 @@
+orocos_component(paper_example component.cpp)
+target_link_libraries(paper_example ${catkin_LIBRARIES})

rtcf_examples/src/paper_example/component.cpp

@@ -0,0 +1,38 @@
+#include <std_msgs/Float32.h>
+
+#include <rtcf/macros.hpp>
+#include <rtcf/rtcf_extension.hpp>
+OROCOS_HEADERS_BEGIN
+#include <rtt/Component.hpp>
+#include <rtt/Port.hpp>
+#include <rtt/RTT.hpp>
+OROCOS_HEADERS_END
+
+class PaperExample : public RTT::TaskContext, public RtcfExtension {
+  public:
+    PaperExample(std::string const& name) : TaskContext(name), port_out_("out_port"), port_in_("in_port") {}
+
+    bool configureHook() {
+        this->ports()->addPort(port_in_);
+        this->ports()->addPort(port_out_);
+        double param = this->getNodeHandle().param("/test_parameter", 0.0);
+        NON_RT_INFO_STREAM("Fetched param with value " << param);
+        return true;
+    }
+    void updateHook() {
+        RT_INFO("Update hook called!");
+        port_in_.read(msg_);
+        port_out_.write(msg_);
+    }
+
+    bool startHook() { return true; }
+    void stopHook() {}
+    void cleanupHook() {}
+
+  private:
+    RTT::OutputPort<std_msgs::Float32> port_out_;
+    RTT::InputPort<std_msgs::Float32> port_in_;
+    std_msgs::Float32 msg_;
+};
+
+ORO_CREATE_COMPONENT(PaperExample)