Merge branch 'master' into develop/pr2

.github/workflows/config.yml

@@ -20,7 +20,7 @@ jobs:
             ROS_PARALLEL_TEST_JOBS: "-j8"
             # latest catkin_virtualenv with pip==21.0.1 is incompatible with python 2.x
             # https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/237
-            BEFORE_SCRIPT : "sudo pip install virtualenv==15.1.0"
+            BEFORE_SCRIPT : "sudo pip install virtualenv==15.1.0 setuptools==44.1.1"
           - ROS_DISTRO: kinetic
             CONTAINER: ubuntu:16.04
             ROS_PARALLEL_TEST_JOBS: "-j8"
@@ -56,10 +56,11 @@ jobs:
           fi
 
       - name: Chcekout
-        uses: actions/checkout@v2
+        uses: actions/checkout@v3.0.2
 
       - name: Start X server
         run: |
+          if [[ "${{ matrix.CONTAINER }}" =~ "jskrobotics/ros-ubuntu:14.04" ]]; then exit 0; fi
           echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections || echo "failing on ros-ubuntu is OK" # set non interactive tzdata https://stackoverflow.com/questions/8671308/non-interactive-method-for-dpkg-reconfigure-tzdata
           sudo apt-get -y -qq install mesa-utils x11-xserver-utils xserver-xorg-video-dummy wget
           export DISPLAY=:0
@@ -68,6 +69,7 @@ jobs:
           sleep 3 # wait x server up
           export QT_X11_NO_MITSHM=1 # http://wiki.ros.org/docker/Tutorials/GUI
           xhost +local:root
+        shell: bash
 
       - name: Run jsk_travis
         uses: jsk-ros-pkg/jsk_travis@master
@@ -82,3 +84,77 @@ jobs:
           TEST_PKGS : ${{ matrix.TEST_PKGS }}
           BEFORE_SCRIPT : ${{ matrix.BEFORE_SCRIPT }}
           EXTRA_DEB : ${{ matrix.EXTRA_DEB }}
+
+
+  ubuntu:
+    runs-on: ubuntu-latest
+
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - DISTRO: ubuntu:22.04
+
+    container: ${{ matrix.DISTRO }}
+
+    steps:
+      - name: Chcekout Source
+        uses: actions/[email protected]
+
+      - name: Install Buildtools
+        run: |
+          set -x
+          echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
+          apt update -q
+          apt install -y -q -qq catkin git curl build-essential libboost-all-dev python3-pip python3-venv python3-rosdep2
+          apt install -y -q -qq ros-desktop-dev rosbash
+          apt install -y -q -qq libself-test-dev libpcl-ros-dev
+          apt install -y -q -qq ros-core-dev ros-robot-dev
+          pip install vcstool
+          rosdep update
+
+      - name: Setup rosinstall_generator
+        run: |
+          set -x
+          # install rosinstall_geneartor with --depend-type buildtool build, see  https://github.com/ros-infrastructure/rosinstall_generator/pull/81
+          git clone https://github.com/k-okada/rosinstall_generator -b add_depend_type
+          cd rosinstall_generator
+          python3 ./setup.py install
+
+      - name: Setup Dependencies Workspace
+        run: |
+          set -x
+          # install dependencies
+          mkdir -p ~/ws_depend/src
+          cd ~/ws_depend/src
+          ROS_PACKAGE_PATH=/usr/share rosinstall_generator --rosdistro noetic --from-path $GITHUB_WORKSPACE --deps --exclude RPP --depend-type buildtool build | tee repos
+          vcs import --shallow < repos
+          # override to use latest development for 22.04
+          rosinstall_generator laser_filters laser_assembler map_server --rosdistro noetic  --upstream-development  | vcs import --force
+          rosinstall_generator catkin_virtualenv --rosdistro noetic | vcs import --force
+          rm -fr jsk_common
+          curl -s -L -O https://patch-diff.githubusercontent.com/raw/locusrobotics/catkin_virtualenv/pull/89.diff
+          patch -p1 < 89.diff
+          rosdep install -qq -r -y --from-path . --ignore-src || echo "OK"
+          cd ..
+          catkin_make_isolated --cmake-args -DCATKIN_ENABLE_TESTING=OFF
+
+      - name: Setup Workspace
+        run: |
+          set -x
+          # hack!
+          apt install -y -q -qq python-is-python3
+          sed -i s/noetic/Debian/ $GITHUB_WORKSPACE/jsk_rosbag_tools/CMakeLists.txt
+          # setup workspace
+          mkdir -p ~/ws_current/src
+          cd ~/ws_current/src
+          ln -sf $GITHUB_WORKSPACE .
+          rosdep install -qq -r -y --from-path . --ignore-src || echo "OK"
+
+      - name: Compile Packages
+        run: |
+          set -x
+          cd ~/ws_current/
+          source ~/ws_depend/devel_isolated/setup.bash
+          catkin_make_isolated --cmake-args -DCATKIN_ENABLE_TESTING=OFF
+        shell: bash

doc/jsk_tools/test_nodes/test_topic_published.rst

@@ -22,6 +22,11 @@ Parameters
 
   If it's ``True``, it tests if **not** published.
 
+- ``~import`` (``List[str]``, Default: ``[]``, Optional)
+
+  List of Python modules to import and use in ``~condition_%d`` expression.
+  For commodity rospy and numpy modules are imported by default.
+
 - ``~condition_%d`` (``str``, Default: ``None``, Optional)
 
   Check bool value condition using the given Python expression.
@@ -31,18 +36,26 @@ Parameters
   For example, topic is ``std_msgs/String`` and if you want to check whether a sentence is a ``hello``, you can do the following.
 
   .. code-block:: bash
+
     condition_0: m.data == 'hello'
 
   If you want to check the frame id of the header, you can do the following.
 
   .. code-block:: bash
+
     condition_0: m.header.frame_id in ['base', 'base_link']
 
   Note that, use escape sequence when using the following symbols ``<(&lt;)``, ``>(&gt;)``, ``&(&amp;)``, ``'(&apos;)`` and ``"(&quot;)``.
 
   .. code-block:: bash
+
     condition_0: m.data &lt; 'spbm'
 
+  The modules given to ``~import`` can also be used.
+
+  .. code-block:: bash
+
+    condition_0: 'numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) &lt; 10.0'
 
 Example
 -------

doc/jsk_topic_tools/class/hz_measure_nodelet.md

@@ -26,9 +26,13 @@ If target `hz` is smaller than `~warning_hz`, this node outputs `diagnostics` at
     Diagnostic messages.
 
 ## Parameter
-- `~message_num` (`Int`, default: `10`):
+- `~measure_time` (`Double`, default: `1.0`):
 
-    Calculate `hz` from the arrival times of `~message_num` topics.
+    Calculate `hz` from the number of topics received in time in `~measure_time`.
+
+- `~message_num` (`Int`, default: `-1`):
+
+    Calculate `hz` from the arrival times of `~message_num` topics. Note that if this value is less than 0 or not set, `~measure_time` will be used.
 
 - `~warning_hz` (`Double`, default: `-1`):
 

doc/jsk_topic_tools/lib/constant_rate_throttle_nodelet.md

@@ -0,0 +1,26 @@
+# constant_rate_throttle_nodelet
+
+![images/lightweight_throttle_nodelet_diagram.svg](images/lightweight_throttle_nodelet_diagram.svg)
+
+## Description
+
+This nodelet provides function like `lightweight_throttle_nodelet`, but support accurate publish rate.
+
+The rate of throttled message is configurable by passing `~update rate` parameter on launching this nodelet.
+
+## Subscribing Topic
+- `~input` (`AnyMsg`):
+
+  Input topic. This topic is throttled to low publish rate.
+
+## Publishing Topic
+- `~output` (`AnyMsg`):
+
+  Throttled topic.
+  Publish rate of throttled topic is configurable by setting `~update_rate` parameter.
+  If `~update_rate` is higher than input topic rate, published message is used from buffer.
+
+## Parameter
+- `~update_rate` (Double, default: `1.0`):
+
+  Publish rate of throttled message [Hz]. This parameter is updated only on launching this nodelet.

doc/jsk_topic_tools/scripts/boolean_node.rst

@@ -45,6 +45,11 @@ Publishing Topics
 Parameters
 ----------
 
+- ``~import`` (``List[str]``, Default: ``[]``, Optional)
+
+  List of Python modules to import and use in ``~input{%d}_condition`` expression.
+  For commodity rospy and numpy modules are imported by default.
+
 
 * ``~input{%d}_condition`` (String, Default: ``m.data``)
 
@@ -65,6 +70,21 @@ Parameters
 
     input1_condition: m.header.frame_id in ['base', 'base_link']
 
+  The modules given to ``~import`` can also be used.
+
+  .. code-block:: bash
+
+    condition_0: 'numpy.linalg.norm([m.wrench.force.x, m.wrench.force.y, m.wrench.force.z]) < 10.0'
+
+
+  Note that this condition is evaluated each time a topic is published. For example, a condition that checks whether a certain topic has arrived within one second look like this.
+
+  .. code-block:: bash
+
+    input1_condition: "(rospy.Time.now() - t).to_sec() < 1.0"
+
+  Use escape sequence when using the following symbols <(``&lt;``), >(``&gt;``), &(``&amp;``), '(``&apos;``) and "(``&quot;``) in launch file.
+
 
 * ``~rate`` (Int, Default: ``100``)
 

jsk_data/src/jsk_data/download_data.py

@@ -96,19 +96,26 @@ def decompress_rosbag(path, quiet=False, chmod=True):
     print('[%s] Finished decompressing the rosbag' % path)
 
 
-def download(client, url, output, quiet=False, chmod=True):
+def download(client, url, output, quiet=False, chmod=True, timeout=30):
     print('[%s] Downloading from %s' % (output, url))
-    cmd = '{client} {url} -O {output}'.format(client=client, url=url,
-                                              output=output)
+    if client == 'wget':
+        cmd = '{client} {url} -O {output} -T {timeout} --tries 1'.format(client=client, url=url,
+                                                                         output=output, timeout=timeout)
+    else:
+        cmd = '{client} {url} -O {output}'.format(client=client, url=url,
+                                                  output=output)
     if quiet:
         cmd += ' --quiet'
     try:
-        subprocess.call(shlex.split(cmd))
+        status = subprocess.call(shlex.split(cmd))
     finally:
         if chmod:
             if not is_file_writable(output):
                 os.chmod(output, 0o766)
-    print('[%s] Finished downloading' % output)
+    if status == 0:
+        print('[%s] Finished downloading' % output)
+    else:
+        print('[%s] Failed downloading. exit_status: %d' % (output, status))
 
 
 def check_md5sum(path, md5):

jsk_network_tools/setup.py

@@ -1,6 +1,6 @@
 ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
 
-from distutils.core import setup
+from setuptools import setup
 from catkin_pkg.python_setup import generate_distutils_setup
 
 # fetch values from package.xml

jsk_ros_patch/image_view2/CMakeLists.txt

@@ -11,12 +11,6 @@ if (CMAKE_VERSION VERSION_LESS 3.4)
   endif(CCACHE_FOUND)
 endif()
 
-if(NOT $ENV{ROS_DISTRO} STRLESS noetic)
-  add_compile_options(-std=c++14)
-elseif(NOT $ENV{ROS_DISTRO} STRLESS kinetic)
-  add_compile_options(-std=c++11)
-endif()
-
 find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure  cv_bridge std_msgs sensor_msgs geometry_msgs image_transport tf image_geometry message_filters message_generation std_srvs pcl_ros)
 
 generate_dynamic_reconfigure_options(

jsk_ros_patch/image_view2/image_view2.cpp

@@ -116,7 +116,11 @@ namespace image_view2{
 
     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> >(local_nh);
     dynamic_reconfigure::Server<Config>::CallbackType f =
+#if __cplusplus < 201100L
       boost::bind(&ImageView2::config_callback, this, _1, _2);
+#else
+      [this](auto& config, auto level){ config_callback(config, level); };
+#endif
     srv_->setCallback(f);
   }
 

jsk_ros_patch/image_view2/points_rectangle_extractor.cpp

@@ -13,6 +13,27 @@
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/PolygonStamped.h>
 
+#if BOOST_VERSION < 106000  // since 1.60.0, boost uses placeholders namesapce for _1,_2...
+#ifndef BOOST_PLAEHOLDERS
+#define BOOST_PLAEHOLDERS
+namespace boost
+{
+  namespace placeholders
+  {
+    extern boost::arg<1> _1;
+    extern boost::arg<2> _2;
+    extern boost::arg<3> _3;
+    extern boost::arg<4> _4;
+    extern boost::arg<5> _5;
+    extern boost::arg<6> _6;
+    extern boost::arg<7> _7;
+    extern boost::arg<8> _8;
+    extern boost::arg<9> _9;
+  }  // namespace placeholders
+}  // namespace boost
+#endif  // BOOST_PLAEHOLDERS
+#endif  // BOOST_VERSION < 106000
+
 class PointsRectExtractor
 {
   typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2,
@@ -43,11 +64,11 @@ class PointsRectExtractor
 
     sync_a_polygon_ = boost::make_shared < message_filters::Synchronizer< PolygonApproxSyncPolicy > > (queue_size);
     sync_a_polygon_->connectInput (points_sub_, rect_sub_);
-    sync_a_polygon_->registerCallback (boost::bind (&PointsRectExtractor::callback_polygon, this, _1, _2));
+    sync_a_polygon_->registerCallback (boost::bind (&PointsRectExtractor::callback_polygon, this, boost::placeholders::_1, boost::placeholders::_2));
 
     sync_a_point_ = boost::make_shared < message_filters::Synchronizer< PointApproxSyncPolicy > > (queue_size);
     sync_a_point_->connectInput (points_sub_, point_sub_);
-    sync_a_point_->registerCallback (boost::bind (&PointsRectExtractor::callback_point, this, _1, _2));
+    sync_a_point_->registerCallback (boost::bind (&PointsRectExtractor::callback_point, this, boost::placeholders::_1, boost::placeholders::_2));
 
     pub_ = pnode_.advertise< sensor_msgs::PointCloud2 > ("output", 1);
   }

jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_video.py

@@ -110,10 +110,15 @@ def bag_to_video(input_bagfile,
 
         # remove 0 time stamp
         stamp = 0.0
-        while stamp == 0.0:
-            stamp, _, img, _ = next(images)
-            if show_progress_bar:
-                progress.update(1)
+        try:
+            while stamp == 0.0:
+                stamp, _, img, _ = next(images)
+                if show_progress_bar:
+                    progress.update(1)
+        except StopIteration:
+            print('[bag_to_video] No timestamp found in all images')
+            print('[bag_to_video] Skipping {}'.format(image_topic))
+            break
         start_stamp = stamp
         width, height = img.shape[1], img.shape[0]
 

jsk_tilt_laser/src/spin_laser_snapshotter.cpp

@@ -103,8 +103,12 @@ class SpinLaserSnapshotter
       private_ns_.param("spindle_frame", spindle_frame_, std::string("multisense/spindle"));
       private_ns_.param("motor_frame", motor_frame_, std::string("multisense/motor"));
       timer_ = private_ns_.createTimer(
-        ros::Duration(1.0 / rate_), boost::bind(
-          &SpinLaserSnapshotter::timerCallback, this, _1));
+        ros::Duration(1.0 / rate_),
+#if __cplusplus < 201100L
+	boost::bind(&SpinLaserSnapshotter::timerCallback, this, _1));
+#else
+        [this](auto& event){ timerCallback(event); });
+#endif
     }
   }
 

jsk_tools/bin/battery_capacity_summary.py

@@ -56,8 +56,8 @@ def output():
     sorted_keys = ["HardwareID"] + sorted(data_column.keys())
     sorted_names = sorted(results)
     fmt = "{:>31}"
-    for i in range(len(data_column.keys())):
-        fmt += "| {:>" + "{w}".format(w=len(data_column.keys()[i])) + "}"
+    for key in data_column.keys():
+        fmt += "| {:>" + "{w}".format(w=len(key)) + "}"
     print(fmt.format("Battery Name", *data_column.keys()))
     for name in sorted_names:
         color = getColor(results[name])