Weekly Meeting 2019-09-10

在我的上一篇文中實作了最簡單的 publisher 和 subscriber，而當中的 callback 是使用 C++11 lambda。

現在就開始作一個簡單的效能分析，目標是量測從一個 node 發布一個字串到另一個 node 上之間的延遲 (latency)，其中會需要找到兩個 callback 的 symbol，因為 C++11 lambda 會是一個 unnamed object，每一個 lambda 都有自己的類型，要找到對應的 lambda 函式執行的位址 (其實是 unnamed object 定義的 operator()) 會相當困難，便改成以 member function 實作。

Notes

Rewrite C++11 lambda callback function into class method.

subscriber:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

#include <cstdio>
#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/string.hpp"

// Create a Listener class that subclasses the generic rclcpp::Node base class.
// The main function below will instantiate the class as a ROS node.
class Listener : public rclcpp::Node
{
public:
  Listener() : Node("listener") {
    sub_ = this->create_subscription<std_msgs::msg::String>(
      "chatter", std::bind(&Listener::callback, this, std::placeholders::_1));
  }

private:
  void callback(const std_msgs::msg::String::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg->data.c_str());
  }

  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Listener>();
  auto node2 = rclcpp::Node::make_shared("talker");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

rclcpp::Node::create_subscription requires callback functions to be type of function object.
function object (or functor) is a class that defines operator()
callback() is non-static member function, so it’s required to pass this pointer.

std::bind(&Listener::callback, this, std::placeholders::_1)

publisher:

在 19 行可依照需求調整發送頻率，我在測量延遲時是用 50ms。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

#include <chrono>
#include <cstdio>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class Talker : public rclcpp::Node {
public:
  Talker()
  : Node("talker"), count_(0) {
    pub_ = this->create_publisher<std_msgs::msg::String>("chatter");
    timerPtr_ = this->create_wall_timer(
      500ms, std::bind(&Talker::callback, this));
  }

private:
  void callback(void) {
    char str[7];
    this->timerPtr_->reset();
    std::snprintf(str, 7, "%06d", this->count_++);
    auto message = std_msgs::msg::String();
    message.data = "Hello, world! " + std::string(str);
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    this->pub_->publish(message);
  }

  int count_;
  rclcpp::TimerBase::SharedPtr timerPtr_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Talker>();
  rclcpp::spin(node);
  rclcpp::shutdown();
}

Measure latency on the same machine

Environment

Distro: Dashing Diademata
OS: 18.04.1 Ubuntu
CPU: Intel® Core™ i5-2520M CPU @ 2.50GHz (2C4T)

$ ls
build  install  log  src

Find out the symbol for callback()

目標是找到 Talker::callback() 在 ELF 檔中對應的 symbol

List all exported symbols in the executable.

$ objdump install/ros_course_demo/lib/ros_course_demo/talker -t
install/ros_course_demo/lib/ros_course_demo/talker:     file format elf64-x86-64

SYMBOL TABLE:
...
0000000000015d40  w    F .text  000000000000004d              _ZN6TalkerD1Ev
0000000000011806  w    F .text  000000000000015b              _ZNSt23_Sp_counted_ptr_inplaceIN6rclcpp9PublisherIN8
std_msgs3msg7String_ISaIvEEES5_EESaIS7_ELN9__gnu_cxx12_Lock_policyE2EEC2IJRPNS0_15node_interfaces17NodeBaseInterfa
ceERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEER23rcl_publisher_options_tRKNS0_23PublisherEventCallbacks
ERSt10shared_ptrISaIS6_EEEEES8_DpOT_
...

C++ uses name mangling to handle classes, templates, namespaces, etc. Use c++filt to demangle symbols:

$ objdump install/ros_course_demo/lib/ros_course_demo/talker -t \
  | c++filt | grep callback
...
0000000000008944  w    F .text  000000000000031c              Talker::callback()
...

Retrieve the symbol in ELF file for Talker::callback() using address 0000000000008944.

$ objdump install/ros_course_demo/lib/ros_course_demo/talker -t \
  | grep 0000000000008944
0000000000008944  w    F .text  000000000000031c              _ZN6Talker8callbackEv

Register probe function

使用剛剛找到的 symbol 並將其帶入 attach_uprobe() 便能完成註冊，一旦 Talker::callback() 被呼叫，註冊的 talker_probe 就會被執行。 更多 uprobe (在 userspace 追蹤程式) 的相關的資訊請看底下 References。

b.attach_uprobe(name="./ros2_course/install/ros_course_demo/lib/ros_course_demo/talker",
                sym="_ZN6Talker8callbackEv", fn_name="talker_probe")

Measure latency

以下為測量方法的時序圖。
首先使用 uprobe 註冊 talker 和 listener 探針函式，分別會在 callback 被呼叫時執行，並測量兩個 callback 之間的時間差。

寫一個 eBPF 的程式追蹤兩個 callback 的呼叫時間：

#!/usr/bin/env python
from __future__ import print_function
from bcc import BPF
from time import sleep

# load BPF program
b = BPF(text="""
#include <uapi/linux/ptrace.h>

BPF_HISTOGRAM(dist);
BPF_ARRAY(start_time, uint64_t, 1);

int talker_probe(struct pt_regs *ctx) {
    uint64_t curr = bpf_ktime_get_ns();
    uint32_t key = 0;

    start_time.update(&key, &curr);
    // bpf_trace_printk("talker_probe curr=%lu\\n", curr);
    return 0;
};

int listener_probe(struct pt_regs *ctx) {
    uint64_t curr = bpf_ktime_get_ns();
    uint64_t *prev, lat;
    uint32_t key = 0;

    prev = start_time.lookup(&key);
    if (prev) {
        lat = (curr - *prev) / 1000;
        dist.increment(bpf_log2l(lat));
        bpf_trace_printk("listener_probe lat=%luusecs\\n", lat);
    }
    return 0;
}
""")

b.attach_uprobe(name="./ros2_course/install/ros_course_demo/lib/ros_course_demo/talker",
                sym="_ZN6Talker8callbackEv", fn_name="talker_probe")
b.attach_uprobe(name="./ros2_course/install/ros_course_demo/lib/ros_course_demo/listener",
                sym="_ZN8Listener8callbackESt10shared_ptrIN8std_msgs3msg7String_ISaIvEEEE",
                fn_name="listener_probe")

while 1:
    try:
        try:
            (task, pid, cpu, flags, ts, msg) = b.trace_fields()
        except ValueError:
            continue
        print("%-18.9f %-16s %-6d %s" % (ts, task, pid, msg))
        # print("%-16s %-6d %s" % (task, pid, msg))
    except KeyboardInterrupt:
        break

print("")
b["dist"].print_log2_hist("usec")

以下為統計結果，每次均 publish 一個長度為 21 的字串。

Future work

STM32MP157A-DK1

STM32MP157 dual Cortex®-A7 32 bits + Cortex®-M4 32 bits MPU

openSTLinux kernel 4.19

Build a PREEMPT_RT kernel for stm32mp1
Cross compile ROS2 for ARMv7

References

BPF In Depth: Communicating with Userspace

https://blogs.oracle.com/linux/notes-on-bpf-3

kprobes

https://www.kernel.org/doc/Documentation/kprobes.txt

uprobe-tracer

https://www.kernel.org/doc/Documentation/trace/uprobetracer.txt

Meet cute-between-ebpf-and-tracing

https://www.slideshare.net/vh21/meet-cutebetweenebpfandtracing

iovisor/bcc

https://github.com/iovisor/bcc

Draft: https://hackmd.io/@1IzBzEXXRsmj6-nLXZ9opw/rJC0fc_2V

這裡已預設 ROS2 已安裝好，不論使用 apt 安裝或者是 github 上的 archive 都可以，且已設置好必要的環境變數 (setup.bash 腳本)
這邊使用 dashing distro。

Create ROS2 workspace

create an empty package:

$ ros2 pkg create --dependencies [deps]

以下為具有兩個 package 的 workspace
directory tree:

.
└── src
    ├── ros_course_demo
    │   ├── CMakeLists.txt
    │   ├── package.xml
    │   └── src
    │       ├── listener.cpp
    │       └── talker.cpp
    └── package_name
        ├── CMakeLists.txt
        ├── package.xml
        └── src
            ├── file1.cpp
            └── file2.cpp

不同於 ROS1 使用 catkin 作為 build system，ROS2 則是使用 ament_cmake，但是目前比較常使用另一個工具叫 colcon 可以同時組建 ROS1 和 ROS2 的 package，輸入

$ colcon build

colcon 就會走訪 src 目錄裡的所有 package 並組建。

$ ros2 run package_name executable_name

install(TARGETS ${target}
  DESTINATION lib/${PROJECT_NAME})

Write a publisher and a subscriber

搭配 rclcpp example 與 rclcpp library api，便能開始使用 ROS2，參考連結均放在底下。

subscriber:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

#include <cstdio>
#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

// Create a Listener class that subclasses the generic rclcpp::Node base class.
// The main function below will instantiate the class as a ROS node.
class Listener : public rclcpp::Node
{
public:
  Listener() : Node("listener")
  {
    auto callback = [this](const std_msgs::msg::String::SharedPtr msg) -> void {
      RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg->data.c_str());
    };
    sub_ = create_subscription<std_msgs::msg::String>("chatter", callback);
  }

private:
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Listener>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

首先必須先建立一個 node，

class Listener 使用繼承自 rclcpp::Node，並在 Node 的 constructor 給予這個 node 的名稱。也可以使用 rclcpp::Node::make_shared() 建立一個 std::shared_ptr<rclcpp::Node> 類型的物件。

接下來由 rclcpp::Node::create_subscription() 註冊 callback function，此 callback 會在呼叫 rclcpp::spin() 時被執行。

當無法使用 auto 型態宣告變數，而需要完整型態時，要使用 rclcpp 定義好的 SharedPtr 來管理物件，以下為例 : “…” 為訂閱 topic 的資料型態。

using rclcpp::Subscription<...>::SharedPtr = std::shared_ptr<...>

publisher:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

#include <chrono>
#include <cstdio>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class Talker : public rclcpp::Node
{
public:
  Talker()
  : Node("talker"), count_(0) {
    pub_ = this->create_publisher<std_msgs::msg::String>("chatter");
    timerPtr_ = this->create_wall_timer(1s,
      [this]() {
        this->timerPtr_->reset();
        auto message = std_msgs::msg::String();
        message.data = "Hello, world! " + std::to_string(this->count_++);
        RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
        this->pub_->publish(message);
      }
    );
  }

private:
  int count_;
  rclcpp::TimerBase::SharedPtr timerPtr_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Talker>();
  rclcpp::spin(node);
  rclcpp::shutdown();
}

ROS2 兩個主要用來做時間管理的工具

1. rclcpp::WallRate

較容易使用，設定固定一段時間並睡眠，會跟 while (rclcpp::ok()) {...} 一起使用
e.g.

rclcpp::WallRate loop_rate(500ms);
loop_rate.sleep();

2. rclcpp::timer::WallTimer

建立一個計時器並立刻倒數。
e.g.

node = rclcpp::Node::make_shared("node_name");

auto timer_ptr = this->create_wall_timer(1s,
  []() { RCLCPP_INFO(node->get_logger(), "node_loop"); }
);

當你建立一個 WallTimer 物件，Timer 就會開始倒數，並沒有界面可以讓你預設就是關閉，只能建立完之後呼叫 cancel() 或等到 rclcpp::spin() 之前呼叫 reset()。

註冊的 callback 在呼叫 rclcpp::spin() 前都不會被執行。

std::chrono

為何可以使用 1s 當作一秒帶入 create_wall_timer() 呢？因為這邊使用了 C++11 的語法 User-defined literals

Allows integer, floating-point, character, and string literals to produce objects of user-defined type by defining a user-defined suffix.

而以下是在 C++14 std::chrono已經定義好的時間常數。

operator""h
operator""min
operator""s
operator""ms
operator""us
operator""ns

constexpr std::chrono::hours operator ""h(unsigned long long h)
{
    return std::chrono::hours(h);
}

std::chrono::hours is a helper type of std::chrono::duration (C++14)
以下是 std::chrono::hours 的定義：

namespace chrono
{
...
  typedef duration<int64_t, ratio<3600>>  hours;
} // namespace chrono

這些 User-defined literals 也支援 operator overloading 的算術操作、比較運算子等，甚至可以對不同時間間隔做操作 (如 h, min, s)，因為他們都是 duration 的別名，只差在是不同的 template specialization，更詳盡的介紹可以參考 cppreference.com 上關於 std::chrono::duration 的頁面。

以下示範對於不同時間間隔的操作：

#include <chrono>
#include <iostream>
using namespace std::chrono_literals;
int main()
{
    std::chrono::seconds s = 1h 
                           + 2*10min
                           + 70s/10;
    std::cout << "1 hour + 2*10 min + 70/10 sec = " << s.count() << " seconds\n";
    return 0;
}

可以看到以下輸出

1 hour + 2*10 min + 70/10 sec = 4807 seconds

只要加上這一行就可以使用 std::chrono 的 User-defined literals

using namespace std::chrono_literals;

demo

References

rclcpp example : https://github.com/ros2/examples/tree/master/rclcpp
rclcpp library api : http://docs.ros2.org/dashing/api/rclcpp/

Todo list

• Gazebo, SLAM
• ROS2 QoS

Draft: https://hackmd.io/CTiXEKLzTIGQ2mAxdBYxYA

Advertisement :)

• pica - high quality and fast image
  resize in browser.
• babelfish - developer friendly
  i18n with plurals support and easy syntax.

You will like those projects!

h1 Heading 8-)

h2 Heading

h3 Heading

h4 Heading

h5 Heading

h6 Heading

Horizontal Rules

Typographic replacements

Enable typographer option to see result.

© © ® ® ™ ™ § § ±

test… test… test… test?.. test!..

!!! ??? , – —

“Smartypants, double quotes” and ‘single quotes’

Emphasis

This is bold text

This is bold text

This is italic text

This is italic text

Strikethrough

Blockquotes

Blockquotes can also be nested…

…by using additional greater-than signs right next to each other…

…or with spaces between arrows.

Lists

Unordered

• Create a list by starting a line with +, -, or *
• Sub-lists are made by indenting 2 spaces:
  • Marker character change forces new list start:
    • Ac tristique libero volutpat at
    • Facilisis in pretium nisl aliquet
    • Nulla volutpat aliquam velit
• Very easy!

Ordered

1. Lorem ipsum dolor sit amet

2. Consectetur adipiscing elit

3. Integer molestie lorem at massa

4. You can use sequential numbers…

5. …or keep all the numbers as 1.

Start numbering with offset:

57. foo
58. bar

Code

Inline code

Indented code

// Some comments
line 1 of code
line 2 of code
line 3 of code

Block code “fences”

Sample text here...

Syntax highlighting

var foo = function (bar) {
  return bar++;
};

console.log(foo(5));

Tables

Right aligned columns

Links

link text

link with title

Autoconverted link https://github.com/nodeca/pica (enable linkify to see)

Images

Like links, Images also have a footnote style syntax

With a reference later in the document defining the URL location:

Plugins

The killer feature of markdown-it is very effective support of
syntax plugins.

Emojies

Classic markup: :wink: :crush: :cry: :tear: :laughing: :yum:

Shortcuts (emoticons): :-) :-( 8-) ;)

see how to change output with twemoji.

Subscript / Superscript

• 19^th
• H₂O

<ins>

Inserted text

<mark>

Marked text

Footnotes

Footnote 1 link^[1].

Footnote 2 link^[2].

Inline footnote^[3] definition.

Duplicated footnote reference^[2:1].

Definition lists

Term 1

Definition 1
with lazy continuation.

Term 2 with inline markup

Definition 2

  { some code, part of Definition 2 }

Third paragraph of definition 2.

Compact style:

Term 1

Definition 1

Term 2

Definition 2a

Definition 2b

Abbreviations

This is HTML abbreviation example.

It converts “HTML”, but keep intact partial entries like “xxxHTMLyyy” and so on.

Custom containers

info container

for (int i = 0; i < 100; i++) {
    int temp = i;
    do {
        float q = i + (rand() % 100);
        q = sqrt(q);
        printf("%f\n", q);
    } while (i++);
    i = temp;
}

warning container:

for (int i = 0; i < 100; i++) {
    int temp = i;
    do {
        float q = i + (rand() % 100);
        q = sqrt(q);
        printf("%f\n", q);
    } while (i--);
    i = temp;
}

danger container:

for (int i = 0; i < 100; i++) {
    do {
        float q = i + (rand() % 100);
        q = sqrt(q);
        printf("%f\n", q);
    } while (i--);
}

to do list

entry1
entry2

Install

Referring to the installation guide in the Igh maunal. It's pretty simple to install the Igh EtherCAT on raspi. Below records my steps to install Igh.

1. update kernel and download kernel source

$ sudo rpi-update
$ reboot
$ git clone https://github.com/notro/rpi-source
$ python rpi-source

2. If rpi-source failed because of complier version, you need to change to the one that compile kernel. e.g.

$ sudo apt-get install -y gcc-4.8
$ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.6 20
$ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.8 50

3. download ethercat

$ wget https://etherlab.org/download/ethercat/ethercat-1.5.2.tar.bz2
$ tar jxf ethercat-1.5.2.tar.bz2

4. intall automake and kernel source

$ sudo apt-get install automake autoconf libtool
$ sudo apt-get install linux-source

5. start install IgH EtherCAT

$ ./bootstrap
$ ./configure
$ make all modules
$ sudo make modules_install install
$ sudo depmod

Note here: if make install failed due to FPU problem (softfp ABI?), use touch on the failed target.

6. create soft link

$ sudo ln -s /opt/etherlab/etc/init.d/ethercat /etc/init.d/ethercat
$ sudo mkdir /etc/sysconfig
$ sudo cp /opt/etherlab/etc/sysconfig/ethercat /etc/sysconfig/ethercat

7. config ethercat, you need to fill in MASTER0_DEVICE with eth0 MAC address, and fill in DEVICE_MODULES with "generic"

$ ifconfig
$ sudo vim /etc/sysconfig/ethercat

8. finish up

$ sudo echo KERNEL==\"EtherCAT[0-9]*\", MODE=\"0664\" > /etc/udev/rules.d/99-EtherCAT.rules
$ ln -s /opt/etherlab/bin/ethercat /usr/local/bin/ethercat

Backup

After we install the Igh EtherCAT, it might be faster that we use the backup image on the new raspi machine.

Find out which devices are currently available:

$ df -h

use command dmesg to confirm that new device you inserted is SD card. Always make sure that if is the device to read and of is the device to write.

To backup SD card: (it might take a while)

$ sudo dd if=/dev/sdc of=/SDCardBackup.img

To restore the image:

$ sudo dd bs=4M if=/SDCardBackup.img of=/dev/sdc

Before ejecting the SD card, make sure that your Linux PC has completed writing:

$ sudo sync

Don't forget to fill in MAC address of the new machine.

PV2

sous-titres français

La vie, ce voyage. Teinté de solitude, de désespoir.
Dont elle chasse les nuages. Douée, dévouée.
Elle tient toujours toutes ses promesses.
Chaque fin est un nouveau départ
La vie est remplie de surprises.
Mais nous donne toujours une lueur d'espoir.

Laisse le temps passer.
Tes plaies vont se fermer.
Car rien n'est plus précieux que l'amour.
Noble, fidèle. Elle est aussi pure que la neige.
Son cœur est velours.

Elle parcourt son destin.
Dans ses yeux pas l'ombre d'un doute.
Une brebis fragile.
Seule face aux périls.
Mais qui ne connaît pas la paix
Sans forcer sa douceur.

Sans cesse désemparée.
Cherchant le grand amour.
Sa vie est aussi douce qu'un baiser.
En paix, enchantée,
Sèche tes larmes pour t'envoler.
Son cœur est velours.

Laisse le temps passer.
Tes plaies vont se fermer.
Car rien n'est plus précieux que l'amour.
Noble, fidèle. Elle est aussi pure que la neige.
Son cœur est velours.