Add support for Visual element

.github/workflows/build.yml

@@ -34,7 +34,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        os: [ubuntu-16.04, ubuntu-18.04, macos-latest]
+        os: [ubuntu-18.04, ubuntu-20.04, macos-latest]
         compiler: [gcc]
     runs-on: ${{ matrix.os }}
     steps:
@@ -46,38 +46,37 @@ jobs:
       run: |
         set -e
         sudo rm -f /etc/apt/sources.list.d/dotnetdev.list /etc/apt/sources.list.d/microsoft-prod.list
+    - name: Setup extra APT mirror
+      if: startsWith(runner.os, 'Linux')
+      run: |
+        set -x
+        set -e
+        curl -1sLf 'https://dl.cloudsmith.io/public/mc-rtc/head/setup.deb.sh' | sudo -E bash
     - name: Install ROS
-      if: (matrix.os == 'ubuntu-16.04' || matrix.os == 'ubuntu-18.04')
+      if: startsWith(runner.os, 'Linux')
       run: |
         set -e
         set -x
         pushd .
         if [ "${{ matrix.os }}" = "ubuntu-16.04" ]
         then
           export ROS_DISTRO="kinetic"
-        else
+        elif [ "${{ matrix.os }}" = "ubuntu-18.04" ]
+        then
           export ROS_DISTRO="melodic"
+        else
+          export ROS_DISTRO="noetic"
         fi
         sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
         wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
         sudo apt-get update -qq
-        sudo apt-get install -qq ros-${ROS_DISTRO}-ros-base ros-${ROS_DISTRO}-tf2-ros ros-${ROS_DISTRO}-tf ros-${ROS_DISTRO}-rviz
+        sudo apt-get install -qq ros-${ROS_DISTRO}-ros-base ros-${ROS_DISTRO}-tf2-ros ros-${ROS_DISTRO}-tf ros-${ROS_DISTRO}-rviz ros-${ROS_DISTRO}-mc-rtc-plugin
         . /opt/ros/${ROS_DISTRO}/setup.bash
         mkdir -p /tmp/_ci/catkin_ws/src/
         cd /tmp/_ci/catkin_ws/src
         catkin_init_workspace
-        git clone --recursive https://github.com/jrl-umi3218/mc_rtc_msgs
-        git clone --recursive https://github.com/jrl-umi3218/mc_rtc_data
         cd ../
         catkin_make || exit 1
-        . devel/setup.bash
-        echo "ROS_DISTRO=${ROS_DISTRO}" >> $GITHUB_ENV
-        echo "PKG_CONFIG_PATH=${PKG_CONFIG_PATH}" >> $GITHUB_ENV
-        echo "ROS_MASTER_URI=${ROS_MASTER_URI}" >> $GITHUB_ENV
-        echo "PYTHONPATH=${PYTHONPATH}" >> $GITHUB_ENV
-        echo "CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}" >> $GITHUB_ENV
-        echo "PATH=${PATH}" >> $GITHUB_ENV
-        echo "LD_LIBRARY_PATH=${LD_LIBRARY_PATH}" >> $GITHUB_ENV
         popd
     - name: Setup env
       if: matrix.os == 'macos-latest'
@@ -92,44 +91,28 @@ jobs:
         compiler: gcc
         build-type: RelWithDebInfo
         ubuntu: |
-          apt: cython cython3 python-nose python3-nose python-numpy python3-numpy python-coverage python3-coverage python-setuptools python3-setuptools libeigen3-dev doxygen doxygen-latex libboost-all-dev libtinyxml2-dev libgeos++-dev libnanomsg-dev libyaml-cpp-dev libltdl-dev qt5-default libqwt-qt5-dev
+          apt: libltdl-dev qt5-default libqwt-qt5-dev libmc-rtc-dev
         macos: |
-          brew: eigen boost tinyxml2 geos nanomsg yaml-cpp pkg-config libtool qt5 qwt gcc
-          pip: Cython coverage nose numpy
-          github:
-            - path: jrl-umi3218/mc_rtc_data
-        github: |
-          - path: gabime/spdlog
-            ref: v1.6.1
-            options: -DSPDLOG_BUILD_EXAMPLE:BOOL=OFF -DSPDLOG_BUILD_SHARED:BOOL=ON
-          - path: humanoid-path-planner/hpp-spline
-            ref: v4.7.0
-            options: -DBUILD_PYTHON_INTERFACE:BOOL=OFF
-          - path: jrl-umi3218/Eigen3ToPython
-          - path: jrl-umi3218/SpaceVecAlg
-          - path: jrl-umi3218/sch-core
-          - path: jrl-umi3218/eigen-qld
-          - path: jrl-umi3218/eigen-quadprog
-          - path: jrl-umi3218/sch-core-python
-          - path: jrl-umi3218/RBDyn
-          - path: jrl-umi3218/Tasks
-          - path: jrl-umi3218/mc_rbdyn_urdf
-          - path: jrl-umi3218/state-observation
-          - path: jrl-umi3218/mc_rtc
+          brew-taps: mc-rtc/mc-rtc
+          brew: qt5 qwt mc_rtc
+          github: # Get the head version of mc_rtc
+            - path: jrl-umi3218/mc_rtc
+              options: -DPYTHON_BINDING:BOOL=OFF
     - name: Build and test
       if: matrix.os == 'macos-latest'
       uses: jrl-umi3218/github-actions/build-cmake-project@master
       with:
         compiler: gcc
         build-type: RelWithDebInfo
     - name: Build with catkin
-      if: (matrix.os == 'ubuntu-16.04' || matrix.os == 'ubuntu-18.04')
+      if: startsWith(runner.os, 'Linux')
       run: |
         set -e
         set -x
         PROJECT_DIR=`pwd`
         cd /tmp/_ci/catkin_ws
         cp -r $PROJECT_DIR src/
+        . devel/setup.bash
         catkin_make || exit 1
     - name: Slack Notification
       if: failure()
@@ -138,4 +121,4 @@ jobs:
         slack-bot-user-oauth-access-token: ${{ secrets.SLACK_BOT_TOKEN }}
         slack-channel: '#ci'
         slack-text: >
-          [mc_rtc] Build *${{ matrix.os }}/${{ matrix.build-type }} (${{ matrix.compiler }})* failed on ${{ github.ref }}
+          [mc_rtc_ros] Build *${{ matrix.os }}/${{ matrix.build-type }} (${{ matrix.compiler }})* failed on ${{ github.ref }}

mc_rtc_rviz_panel/src/CMakeLists.txt

@@ -28,6 +28,7 @@ if(NOT ${DISABLE_ROS})
     ForceInteractiveMarkerWidget.h
     Point3DInteractiveMarkerWidget.h
     TransformInteractiveMarkerWidget.h
+    VisualWidget.h
     XYThetaInteractiveMarkerWidget.h
     DisplayTrajectoryWidget.h
     PolygonMarkerWidget.h
@@ -137,6 +138,7 @@ if(NOT ${DISABLE_ROS})
     ForceInteractiveMarkerWidget.cpp
     Point3DInteractiveMarkerWidget.cpp
     TransformInteractiveMarkerWidget.cpp
+    VisualWidget.cpp
     XYThetaInteractiveMarkerWidget.cpp
     DisplayTrajectoryWidget.cpp
     PolygonMarkerWidget.cpp

mc_rtc_rviz_panel/src/Panel.cpp

@@ -20,6 +20,7 @@
 #  include "Point3DInteractiveMarkerWidget.h"
 #  include "PolygonMarkerWidget.h"
 #  include "TransformInteractiveMarkerWidget.h"
+#  include "VisualWidget.h"
 #  include "XYThetaInteractiveMarkerWidget.h"
 #endif
 #include "ConnectionDialog.h"
@@ -169,6 +170,7 @@ Panel::Panel(QWidget * parent)
   qRegisterMetaType<std::vector<sva::PTransformd>>("std::vector<sva::PTransformd>");
   qRegisterMetaType<std::vector<std::vector<Eigen::Vector3d>>>("std::vector<std::vector<Eigen::Vector3d>>");
   qRegisterMetaType<std::vector<std::vector<double>>>("std::vector<std::vector<double>>");
+  qRegisterMetaType<rbd::parsers::Visual>("rbd::parsers::Visual");
   tree_.parent = this;
   setContextMenuPolicy(Qt::CustomContextMenu);
   connect(this, SIGNAL(customContextMenuRequested(const QPoint &)), this, SLOT(contextMenu(const QPoint &)));
@@ -256,6 +258,8 @@ Panel::Panel(QWidget * parent)
           this,
           SLOT(got_robot(const WidgetId &, const std::vector<std::string> &, const std::vector<std::vector<double>> &,
                          const sva::PTransformd &)));
+  connect(this, SIGNAL(signal_visual(const WidgetId &, const rbd::parsers::Visual &, const sva::PTransformd &)), this,
+          SLOT(got_visual(const WidgetId &, const rbd::parsers::Visual &, const sva::PTransformd &)));
   connect(this, SIGNAL(signal_form(const WidgetId &)), this, SLOT(got_form(const WidgetId &)));
   connect(this, SIGNAL(signal_form_checkbox(const WidgetId &, const std::string &, bool, bool)), this,
           SLOT(got_form_checkbox(const WidgetId &, const std::string &, bool, bool)));
@@ -522,6 +526,11 @@ void Panel::robot(const WidgetId & id,
   Q_EMIT signal_robot(id, parameters, q, posW);
 }
 
+void Panel::visual(const WidgetId & id, const rbd::parsers::Visual & visual, const sva::PTransformd & pos)
+{
+  Q_EMIT signal_visual(id, visual, pos);
+}
+
 void Panel::form(const WidgetId & id)
 {
   Q_EMIT signal_form(id);
@@ -858,6 +867,14 @@ void Panel::got_robot(const WidgetId & /*id*/,
 {
 }
 
+void Panel::got_visual(const WidgetId & id, const rbd::parsers::Visual & visual, const sva::PTransformd & pose)
+{
+#ifndef DISABLE_ROS
+  auto & w = get_widget<VisualWidget>(id, impl_->marker_array_);
+  w.update(visual, pose);
+#endif
+}
+
 void Panel::got_form(const WidgetId & id)
 {
   auto & form = get_widget<FormWidget>(id);

mc_rtc_rviz_panel/src/Panel.h

@@ -154,6 +154,8 @@ class Panel : public CategoryWidget, public mc_control::ControllerClient
              const std::vector<std::vector<double>> & q,
              const sva::PTransformd & posW) override;
 
+  void visual(const WidgetId & id, const rbd::parsers::Visual & visual, const sva::PTransformd & pose) override;
+
   void form(const WidgetId & id) override;
 
   void form_checkbox(const WidgetId & formId, const std::string & name, bool required, bool def) override;
@@ -303,6 +305,7 @@ private slots:
                  const std::vector<std::string> & parameters,
                  const std::vector<std::vector<double>> & q,
                  const sva::PTransformd & posW);
+  void got_visual(const WidgetId & id, const rbd::parsers::Visual & visual, const sva::PTransformd & pose);
   void got_form(const WidgetId & id);
   void got_form_checkbox(const WidgetId & formId, const std::string & name, bool required, bool def);
   void got_form_integer_input(const WidgetId & formId, const std::string & name, bool required, int def);
@@ -404,6 +407,7 @@ private slots:
                     const std::vector<std::string> & parameters,
                     const std::vector<std::vector<double>> & q,
                     const sva::PTransformd & posW);
+  void signal_visual(const WidgetId & id, const rbd::parsers::Visual & visual, const sva::PTransformd & pose);
   void signal_form(const WidgetId & id);
   void signal_form_checkbox(const WidgetId & formId, const std::string & name, bool required, bool def);
   void signal_form_integer_input(const WidgetId & formId, const std::string & name, bool required, int def);

mc_rtc_rviz_panel/src/VisualWidget.cpp

@@ -0,0 +1,131 @@
+#include "VisualWidget.h"
+
+namespace mc_rtc_rviz
+{
+
+VisualWidget::VisualWidget(const ClientWidgetParam & params, visualization_msgs::MarkerArray & markers)
+: ClientWidget(params), markers_(markers), visible_(visible()), was_visible_(visible_)
+{
+  auto layout = new QHBoxLayout(this);
+  if(!secret())
+  {
+    layout->addWidget(new QLabel(id().name.c_str()));
+  }
+  button_ = new QPushButton(this);
+  button_->setCheckable(true);
+  button_->setChecked(!visible());
+  toggled(!visible_);
+  layout->addWidget(button_);
+  connect(button_, SIGNAL(toggled(bool)), this, SLOT(toggled(bool)));
+  marker_.ns = id2name(id());
+  marker_.header.frame_id = "robot_map";
+  marker_.header.seq = 0;
+}
+
+VisualWidget::~VisualWidget()
+{
+  marker_.action = visualization_msgs::Marker::DELETE;
+  markers_.markers.push_back(marker_);
+}
+
+void VisualWidget::update(const rbd::parsers::Visual & visual, const sva::PTransformd & pose)
+{
+  {
+    // Marker general settings
+    marker_.header.seq++;
+    marker_.header.stamp = ros::Time::now();
+    marker_.action = visualization_msgs::Marker::ADD;
+  }
+  {
+    // Convert the position
+    auto X_0_marker = visual.origin * pose;
+    geometry_msgs::Pose p;
+    Eigen::Quaterniond q{X_0_marker.rotation().transpose()};
+    p.orientation.w = q.w();
+    p.orientation.x = q.x();
+    p.orientation.y = q.y();
+    p.orientation.z = q.z();
+    const auto & t = X_0_marker.translation();
+    p.position.x = t.x();
+    p.position.y = t.y();
+    p.position.z = t.z();
+    marker_.pose = p;
+  }
+  {
+    // Color from material, defaults to white
+    if(visual.material.type == rbd::parsers::Material::Type::COLOR)
+    {
+      auto & c = boost::get<rbd::parsers::Material::Color>(visual.material.data);
+      marker_.color.r = c.r;
+      marker_.color.g = c.g;
+      marker_.color.b = c.b;
+      marker_.color.a = c.a;
+    }
+    else
+    {
+      marker_.color.r = 1.0;
+      marker_.color.g = 1.0;
+      marker_.color.b = 1.0;
+      marker_.color.a = 1.0;
+    }
+  }
+  {
+    // Convert geometry
+    using Geometry = rbd::parsers::Geometry;
+    using Type = Geometry::Type;
+    auto & type = visual.geometry.type;
+    auto & data = visual.geometry.data;
+    if(type == Type::BOX)
+    {
+      auto & b = boost::get<Geometry::Box>(data);
+      marker_.type = visualization_msgs::Marker::CUBE;
+      marker_.scale.x = b.size.x();
+      marker_.scale.y = b.size.y();
+      marker_.scale.z = b.size.z();
+    }
+    else if(type == Type::CYLINDER)
+    {
+      auto & c = boost::get<Geometry::Cylinder>(data);
+      marker_.type = visualization_msgs::Marker::CYLINDER;
+      marker_.scale.x = 2 * c.radius;
+      marker_.scale.y = 2 * c.radius;
+      marker_.scale.z = c.length;
+    }
+    else if(type == Type::SPHERE)
+    {
+      auto & s = boost::get<Geometry::Sphere>(data);
+      marker_.type = visualization_msgs::Marker::SPHERE;
+      marker_.scale.x = 2 * s.radius;
+      marker_.scale.y = 2 * s.radius;
+      marker_.scale.z = 2 * s.radius;
+    }
+    else if(type == Type::MESH)
+    {
+      auto & m = boost::get<Geometry::Mesh>(data);
+      marker_.type = visualization_msgs::Marker::MESH_RESOURCE;
+      marker_.scale.x = m.scale;
+      marker_.scale.y = m.scale;
+      marker_.scale.z = m.scale;
+      marker_.mesh_resource = m.filename;
+      marker_.mesh_use_embedded_materials = true;
+    }
+  }
+  if(visible_ || was_visible_)
+  {
+    if(!visible_)
+    {
+      marker_.action = visualization_msgs::Marker::DELETE;
+    }
+    markers_.markers.push_back(marker_);
+  }
+  was_visible_ = visible_;
+}
+
+void VisualWidget::toggled(bool hide)
+{
+  visible_ = !hide;
+  button_->setText(hide ? "Show" : "Hide");
+  visible(!hide);
+}
+
+} // namespace mc_rtc_rviz

mc_rtc_rviz_panel/src/VisualWidget.h

@@ -0,0 +1,35 @@
+/*
+ * Copyright 2021 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#pragma once
+
+#include "ClientWidget.h"
+#include "utils.h"
+
+#include <visualization_msgs/MarkerArray.h>
+
+namespace mc_rtc_rviz
+{
+
+struct VisualWidget : public ClientWidget
+{
+  Q_OBJECT
+public:
+  VisualWidget(const ClientWidgetParam & params, visualization_msgs::MarkerArray & markers);
+
+  ~VisualWidget() override;
+
+  void update(const rbd::parsers::Visual & visual, const sva::PTransformd & pose);
+
+private:
+  visualization_msgs::MarkerArray & markers_;
+  visualization_msgs::Marker marker_;
+  bool visible_;
+  bool was_visible_;
+  QPushButton * button_;
+private slots:
+  void toggled(bool);
+};
+
+} // namespace mc_rtc_rviz