diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 681d848..7512c8b 100644 --- a/.github/workflows/build.yml +++ b/.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,8 +46,14 @@ 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 @@ -55,29 +61,22 @@ jobs: 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,30 +91,13 @@ 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 @@ -123,13 +105,14 @@ jobs: 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 }} diff --git a/mc_rtc_rviz_panel/src/CMakeLists.txt b/mc_rtc_rviz_panel/src/CMakeLists.txt index eedcd8d..93b11c9 100644 --- a/mc_rtc_rviz_panel/src/CMakeLists.txt +++ b/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 diff --git a/mc_rtc_rviz_panel/src/Panel.cpp b/mc_rtc_rviz_panel/src/Panel.cpp index 67add90..0525fbe 100644 --- a/mc_rtc_rviz_panel/src/Panel.cpp +++ b/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"); qRegisterMetaType>>("std::vector>"); qRegisterMetaType>>("std::vector>"); + qRegisterMetaType("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 &, const std::vector> &, 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(id, impl_->marker_array_); + w.update(visual, pose); +#endif +} + void Panel::got_form(const WidgetId & id) { auto & form = get_widget(id); diff --git a/mc_rtc_rviz_panel/src/Panel.h b/mc_rtc_rviz_panel/src/Panel.h index 6544a8d..571c480 100644 --- a/mc_rtc_rviz_panel/src/Panel.h +++ b/mc_rtc_rviz_panel/src/Panel.h @@ -154,6 +154,8 @@ class Panel : public CategoryWidget, public mc_control::ControllerClient const std::vector> & 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 & parameters, const std::vector> & 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 & parameters, const std::vector> & 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); diff --git a/mc_rtc_rviz_panel/src/VisualWidget.cpp b/mc_rtc_rviz_panel/src/VisualWidget.cpp new file mode 100644 index 0000000..653b6d7 --- /dev/null +++ b/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(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(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(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(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(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 diff --git a/mc_rtc_rviz_panel/src/VisualWidget.h b/mc_rtc_rviz_panel/src/VisualWidget.h new file mode 100644 index 0000000..f822840 --- /dev/null +++ b/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 + +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