[jsk_topic_tools/filtered_relay] Add test and document

Author: iory

doc/jsk_topic_tools/scripts/filtered_relay.md

@@ -0,0 +1,65 @@
+# filtered_relay.py
+
+## Description
+
+`rostopic echo` has a filter function, but it cannot publish a topic.
+`topic_tools/transform` can format a topic and publish it, but it does not have the ability to filter.
+Transforming while filtering a topic is simple and powerful.
+This node provides a filter and publish (i.e. relay) functions.
+
+## Subscribing Topic
+
+* `~input` (`rospy/AnyMsg`)
+
+  Input message.
+
+## Publishing Topic
+
+* `~output` (`~output_type`)
+
+  Output topic. You can specify `~output_type` param to publish topic.
+
+## Parameters
+
+* `~output_type`  (`String`, required)
+
+  Message type of output_topic like `std_msgs/Float32`. This is the input for the [get_message_class](http://docs.ros.org/en/diamondback/api/roslib/html/python/roslib.message-module.html#get_message_class) function.
+
+* `~filter`  (`String`, default: `None`)
+
+  Condition of messages that match a specified Python expression.
+
+  The Python expression can access any of the Python builtins plus: ``topic`` (the topic of the message), ``m`` (the message) and ``t`` (time of message).
+
+  For example, ``~input`` topic is ``std_msgs/String`` and if you want to check whether a sentence is a ``hello``, you can do the following.
+
+```bash
+filter: m.data == 'hello'
+```
+
+  Note that, use escape sequence when using the following symbols ``<(&lt;)``, ``>(&gt;)``, ``&(&amp;)``, ``'(&apos;)`` and ``"(&quot;)`` in launch file.
+
+* `~transform`  (`String`, default: `m`)
+
+  Python expression that transform the input messages, which are given in the variable m. The default expression is `m`, which results in forwarding input (which can be a topic field) into output_topic.
+
+* `~import`  (`List[String]`, default: `[]`)
+
+  List of Python modules to import and use in the expression.
+
+## Usage
+
+```bash
+$ rosrun jsk_topic_tools filtered_relay.py ~input:=/right_endeffector/wrench \
+    ~output:=/right_endeffector/force_norm \
+    _filter:='numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) > 10.0' \
+    _output_type:=geometry_msgs/WrenchStamped _import:="[geometry_msgs, numpy]"
+```
+
+## Example
+
+The following example subscribe to `/right_endeffector/wrench` and only those with a force norm greater than 10 are published as `/right_endeffector/force_norm`.
+
+```bash
+roslaunch jsk_topic_tools sample_filtered_relay.launch
+```

jsk_topic_tools/CMakeLists.txt

@@ -166,6 +166,7 @@ if (CATKIN_ENABLE_TESTING)
   endmacro()
   find_package(rostest REQUIRED)
   jsk_topic_tools_add_rostest(test/test_boolean_node.test)
+  jsk_topic_tools_add_rostest(test/test_filtered_relay.test)
   jsk_topic_tools_add_rostest(test/test_topic_buffer.test)
   jsk_topic_tools_add_rostest(test/test_topic_buffer_close_wait.test)
   # topic_buffer tests are tend to fail in GA because of CPU high load.

jsk_topic_tools/sample/sample_filtered_relay.launch

@@ -0,0 +1,57 @@
+<launch>
+
+  <arg name="gui" default="false" />
+  <arg name="launch_robot_model" default="false" />
+
+  <include file="$(find jsk_topic_tools)/sample/sample_transform_wrench.launch" >
+    <arg name="launch_robot_model" value="$(arg launch_robot_model)" />
+    <arg name="gui" value="$(arg gui)" />
+  </include>
+
+  <node name="filtered_relay"
+        pkg="jsk_topic_tools" type="filtered_relay.py"
+        output="screen"
+        clear_params="true" >
+    <remap from="~input" to="right_endeffector/wrench" />
+    <remap from="~output" to="right_endeffector/force_norm" />
+    <rosparam>
+      output_type: std_msgs/Float32
+      import: [std_msgs, geometry_msgs, numpy]
+      filter: "numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) &gt; 10.0"
+      transform: "numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z])"
+    </rosparam>
+  </node>
+
+  <node name="echo_force_norm"
+        pkg="rostopic" type="rostopic"
+        args="echo /right_endeffector/force_norm"
+        output="screen"
+        clear_params="true" >
+  </node>
+
+  <node name="filtered_relay_without_transform"
+        pkg="jsk_topic_tools" type="filtered_relay.py"
+        output="screen"
+        clear_params="true" >
+    <remap from="~input" to="right_endeffector/wrench" />
+    <remap from="~output" to="right_endeffector/filtered_wrench" />
+    <rosparam>
+      output_type: geometry_msgs/WrenchStamped
+      import: [geometry_msgs, numpy]
+      filter: "numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) &gt; 10.0"
+    </rosparam>
+  </node>
+
+  <node name="filtered_relay_without_filter"
+        pkg="jsk_topic_tools" type="filtered_relay.py"
+        output="screen"
+        clear_params="true" >
+    <remap from="~input" to="right_endeffector/wrench" />
+    <remap from="~output" to="right_endeffector/relayed_wrench" />
+    <rosparam>
+      output_type: geometry_msgs/WrenchStamped
+      import: [geometry_msgs]
+    </rosparam>
+  </node>
+
+</launch>

jsk_topic_tools/test/test_filtered_relay.test

@@ -0,0 +1,19 @@
+<launch>
+
+  <include file="$(find jsk_topic_tools)/sample/sample_filtered_relay.launch" >
+  </include>
+
+  <test test-name="test_filtered_relay"
+        name="test_filtered_relay"
+        pkg="jsk_tools" type="test_topic_published.py">
+    <rosparam>
+      topic_0: /right_endeffector/force_norm
+      timeout_0: 30
+      topic_1: /right_endeffector/filtered_wrench
+      timeout_1: 30
+      topic_2: /right_endeffector/relayed_wrench
+      timeout_2: 30
+    </rosparam>
+  </test>
+
+</launch>