Add ConstantRateThrottle Nodelet

[sktometometo]

Fix #1747.
There is still some problem.

This PR provides ConstantRateThrottle Nodelet which throttle a topic to accurate specified rate.

constant_rate_throttle_resized.mp4

to run a demo,

roslaunch jsk_topic_tools sample_constant_rate_throttle.launch

[iory]

Could you add test code?
You can use hztest.
It's suitable for this PR.
http://wiki.ros.org/rostest/Nodes#hztest

[sktometometo]

I'll add a test.

[k-okada]

Will this fix the problaem @haraduka reporeted on 2019/6/25 on 学生教員 mail?

都築くんがこの前の研究会で話したtopic_tools/throttleの挙動がおかしいという件で, sample programを用意してくれました.
https://github.com/tsu-K/throttle_debug

catkin bt
source ~/.bashrc
roslaunch launch/single_array.launch from:=125 to:=50 array_size:=100000

で実行できます.
少なくとも都築と自分のPCでは, size:=100程度だとHzが41くらい, size:=1000程度だとHzが21くらい, size:=100000程度だと逆にHzが41くらいまで戻る, という現象が起こっています.
sizeが極端にでかいor小さければ正常に動き, 1000程度だと一気にHzが下がってしまうという現状です.

https://github.com/jsk-ros-pkg/jsk_tendon_robot/pull/1176 ???

[sktometometo]

@k-okada

With this PR. throttled rate does not fall down to ~20Hz. But there still exists strange behavior.

<launch>

  <arg name="from" default="125" />
  <arg name="to" default="50" />
  <arg name="array_size" default="10" />
  <arg name="stamped" default="False" />
  <arg name="throttle_type" default="normal" />

  <node name="single_array_publisher"
        pkg="throttle_debug" type= "single_array_publisher.py" output="screen">
    <param name="array_size" value="$(arg array_size)" />
    <param name="publish_rate" value="$(arg from)" />
    <param name="with_header" value="$(arg stamped)" />
  </node>

  <node name="tools_throttle_normal"
      pkg="topic_tools" type= "throttle" args="messages /hoge $(arg to)" output="screen"
      if="$(eval arg('throttle_type')=='normal')"/>

  <node name="tools_throttle_constant"
      pkg="nodelet" type="nodelet"
      args="standalone jsk_topic_tools/ConstantRateThrottle" output="screen"
      if="$(eval arg('throttle_type')=='constant_rate')">
      <rosparam subst_value="true">
          update_rate: $(arg to)
      </rosparam>
      <remap from="~input" to="/hoge" />
      <remap from="~output" to="/hoge_throttle" />
  </node>

  <node name="rostopic_hz"
        pkg="rostopic" type="rostopic" args="hz /hoge_throttle -w 10" output="screen"/>

</launch>

array_size = 100

~/ros/ws_jsk_ros_pkg/src/throttle_debug $ roslaunch launch/single_array.launch from:=125 to:=50 array_size:=100 throttle_type:=constant_rate
... logging to /home/sktometometo/.ros/log/076f6d2e-446f-11ed-9a55-7c10c9467f3d/roslaunch-Leopard-16659.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/sktometometo/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://Leopard:42143/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /single_array_publisher/array_size: 100
 * /single_array_publisher/publish_rate: 125
 * /single_array_publisher/with_header: False
 * /tools_throttle_constant/update_rate: 50

NODES
  /
    rostopic_hz (rostopic/rostopic)
    single_array_publisher (throttle_debug/single_array_publisher.py)
    tools_throttle_constant (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [16743]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 076f6d2e-446f-11ed-9a55-7c10c9467f3d
process[rosout-1]: started with pid [16817]
started core service [/rosout]
process[single_array_publisher-2]: started with pid [16824]
process[tools_throttle_constant-3]: started with pid [16831]
type is jsk_topic_tools/ConstantRateThrottle
process[rostopic_hz-4]: started with pid [16838]
WARNING: topic [/hoge_throttle] does not appear to be published yet
subscribed to [/hoge_throttle]
average rate: 49.998
        min: 0.020s max: 0.020s std dev: 0.00013s window: 10
average rate: 50.003
        min: 0.020s max: 0.020s std dev: 0.00010s window: 10

array_size=1000

~/ros/ws_jsk_ros_pkg/src/throttle_debug $ roslaunch launch/single_array.launch from:=125 to:=50 array_size:=1000 throttle_type:=constant_rate
... logging to /home/sktometometo/.ros/log/27873948-446f-11ed-9a55-7c10c9467f3d/roslaunch-Leopard-17233.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/sktometometo/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://Leopard:35997/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /single_array_publisher/array_size: 1000
 * /single_array_publisher/publish_rate: 125
 * /single_array_publisher/with_header: False
 * /tools_throttle_constant/update_rate: 50

NODES
  /
    rostopic_hz (rostopic/rostopic)
    single_array_publisher (throttle_debug/single_array_publisher.py)
    tools_throttle_constant (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [17306]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 27873948-446f-11ed-9a55-7c10c9467f3d
process[rosout-1]: started with pid [17391]
started core service [/rosout]
process[single_array_publisher-2]: started with pid [17398]
process[tools_throttle_constant-3]: started with pid [17405]
type is jsk_topic_tools/ConstantRateThrottle
process[rostopic_hz-4]: started with pid [17411]
WARNING: topic [/hoge_throttle] does not appear to be published yet
subscribed to [/hoge_throttle]
average rate: 61.622
        min: 0.000s max: 0.044s std dev: 0.02008s window: 10
average rate: 51.133
        min: 0.000s max: 0.044s std dev: 0.02184s window: 10
average rate: 51.136
        min: 0.000s max: 0.044s std dev: 0.02184s window: 10

array_size = 10000

~/ros/ws_jsk_ros_pkg/src/throttle_debug $ roslaunch launch/single_array.launch from:=125 to:=50 array_size:=10000 throttle_type:=constant_rate
... logging to /home/sktometometo/.ros/log/3ca9dc4a-446f-11ed-9a55-7c10c9467f3d/roslaunch-Leopard-17803.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/sktometometo/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://Leopard:41497/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /single_array_publisher/array_size: 10000
 * /single_array_publisher/publish_rate: 125
 * /single_array_publisher/with_header: False
 * /tools_throttle_constant/update_rate: 50

NODES
  /
    rostopic_hz (rostopic/rostopic)
    single_array_publisher (throttle_debug/single_array_publisher.py)
    tools_throttle_constant (nodelet/nodelet)

auto-starting new master
process[master]: started with pid [17876]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 3ca9dc4a-446f-11ed-9a55-7c10c9467f3d
process[rosout-1]: started with pid [17950]
started core service [/rosout]
process[single_array_publisher-2]: started with pid [17957]
process[tools_throttle_constant-3]: started with pid [17962]
process[rostopic_hz-4]: started with pid [17970]
type is jsk_topic_tools/ConstantRateThrottle
WARNING: topic [/hoge_throttle] does not appear to be published yet
subscribed to [/hoge_throttle]
average rate: 50.003
        min: 0.000s max: 0.040s std dev: 0.01331s window: 10
average rate: 50.013
        min: 0.000s max: 0.040s std dev: 0.01331s window: 10

I don't know why only 1000 Hz case's average rate is changed...