From 3c04de44e49dd7df5ff8eec140636c066a123817 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 15 Feb 2024 11:26:02 +0900 Subject: [PATCH 01/10] Remove tools that have a "native" version mc_convex_visualization/mc_rtc_ticker/mc_surfaces_visualization have native versions of the same name mc_log_visualization is superseeded by the Replay functionalities We keep the mc_rtc_ticker package for the launch file it provides and is widely used: `roslaunch mc_rtc_ticker display.launch` --- debian/control.ros | 25 +- ...ROS_DISTRO-mc-convex-visualization.install | 3 - ...os-ROS_DISTRO-mc-log-visualization.install | 3 - ...S_DISTRO-mc-surfaces-visualization.install | 3 - mc_convex_visualization/.gitmodules | 3 - mc_convex_visualization/CMakeLists.txt | 33 -- mc_convex_visualization/README.md | 24 - mc_convex_visualization/launch/convex.rviz | 150 ------ .../launch/convex_publisher.launch | 12 - mc_convex_visualization/launch/display.launch | 11 - mc_convex_visualization/package.xml | 24 - mc_convex_visualization/src/main.cpp | 288 ----------- mc_log_visualization/CMakeLists.txt | 35 -- mc_log_visualization/README.md | 33 -- .../launch/log_visualizer.launch | 11 - mc_log_visualization/package.xml | 24 - mc_log_visualization/src/LogPublisher.cpp | 485 ------------------ mc_log_visualization/src/LogPublisher.h | 119 ----- mc_log_visualization/src/LogRobot.cpp | 62 --- mc_log_visualization/src/LogRobot.h | 61 --- .../src/mc_log_visualization.cpp | 73 --- mc_rtc_ticker/CMakeLists.txt | 19 +- mc_rtc_ticker/etc/mc_rtc.conf | 29 -- mc_rtc_ticker/launch/control_display.launch | 21 - mc_rtc_ticker/src/mc_rtc_ticker.cpp | 246 --------- mc_surfaces_visualization/CMakeLists.txt | 35 -- mc_surfaces_visualization/README.md | 30 -- .../launch/display.launch | 13 - .../launch/surfaces.rviz | 152 ------ .../launch/surfaces_publisher.launch | 14 - mc_surfaces_visualization/package.xml | 23 - mc_surfaces_visualization/src/main.cpp | 254 --------- 32 files changed, 3 insertions(+), 2315 deletions(-) delete mode 100644 debian/ros-ROS_DISTRO-mc-convex-visualization.install delete mode 100644 debian/ros-ROS_DISTRO-mc-log-visualization.install delete mode 100644 debian/ros-ROS_DISTRO-mc-surfaces-visualization.install delete mode 100644 mc_convex_visualization/.gitmodules delete mode 100644 mc_convex_visualization/CMakeLists.txt delete mode 100644 mc_convex_visualization/README.md delete mode 100644 mc_convex_visualization/launch/convex.rviz delete mode 100644 mc_convex_visualization/launch/convex_publisher.launch delete mode 100644 mc_convex_visualization/launch/display.launch delete mode 100644 mc_convex_visualization/package.xml delete mode 100644 mc_convex_visualization/src/main.cpp delete mode 100644 mc_log_visualization/CMakeLists.txt delete mode 100644 mc_log_visualization/README.md delete mode 100644 mc_log_visualization/launch/log_visualizer.launch delete mode 100644 mc_log_visualization/package.xml delete mode 100644 mc_log_visualization/src/LogPublisher.cpp delete mode 100644 mc_log_visualization/src/LogPublisher.h delete mode 100644 mc_log_visualization/src/LogRobot.cpp delete mode 100644 mc_log_visualization/src/LogRobot.h delete mode 100644 mc_log_visualization/src/mc_log_visualization.cpp delete mode 100644 mc_rtc_ticker/etc/mc_rtc.conf delete mode 100644 mc_rtc_ticker/src/mc_rtc_ticker.cpp delete mode 100644 mc_surfaces_visualization/CMakeLists.txt delete mode 100644 mc_surfaces_visualization/README.md delete mode 100644 mc_surfaces_visualization/launch/display.launch delete mode 100644 mc_surfaces_visualization/launch/surfaces.rviz delete mode 100644 mc_surfaces_visualization/launch/surfaces_publisher.launch delete mode 100644 mc_surfaces_visualization/package.xml delete mode 100644 mc_surfaces_visualization/src/main.cpp diff --git a/debian/control.ros b/debian/control.ros index 79cd70d..7d93d7b 100644 --- a/debian/control.ros +++ b/debian/control.ros @@ -1,22 +1,4 @@ -Package: ros-@ROS_DISTRO@-mc-convex-visualization -Architecture: any -Section: science -Depends: ${misc:Depends}, - ${shlibs:Depends}, - ros-@ROS_DISTRO@-rviz -Description: mc-rtc - Convex visualization - RViZ based tools to visualize convexes - -Package: ros-@ROS_DISTRO@-mc-log-visualization -Architecture: any -Section: science -Depends: ${misc:Depends}, - ${shlibs:Depends}, - ros-@ROS_DISTRO@-rviz -Description: mc-rtc - Log visualization - RViZ based tools to visualize logs - Package: ros-@ROS_DISTRO@-mc-rtc-rviz-panel Architecture: any Section: science @@ -46,10 +28,7 @@ Description: mc-rtc - Surfaces visualization Package: ros-@ROS_DISTRO@-mc-rtc-tools Architecture: any Section: science -Depends: ros-@ROS_DISTRO@-mc-convex-visualization, - ros-@ROS_DISTRO@-mc-log-visualization, - ros-@ROS_DISTRO@-mc-rtc-rviz-panel, - ros-@ROS_DISTRO@-mc-rtc-ticker, - ros-@ROS_DISTRO@-mc-surfaces-visualization +Depends: ros-@ROS_DISTRO@-mc-rtc-rviz-panel, + ros-@ROS_DISTRO@-mc-rtc-ticker Description: mc-rtc - ROS tools Collection of ROS-based tools for mc_rtc diff --git a/debian/ros-ROS_DISTRO-mc-convex-visualization.install b/debian/ros-ROS_DISTRO-mc-convex-visualization.install deleted file mode 100644 index cbdc895..0000000 --- a/debian/ros-ROS_DISTRO-mc-convex-visualization.install +++ /dev/null @@ -1,3 +0,0 @@ -opt/ros/@ROS_DISTRO@/lib/mc_convex_visualization/* -opt/ros/@ROS_DISTRO@/lib/pkgconfig/mc_convex_visualization.pc -opt/ros/@ROS_DISTRO@/share/mc_convex_visualization/* diff --git a/debian/ros-ROS_DISTRO-mc-log-visualization.install b/debian/ros-ROS_DISTRO-mc-log-visualization.install deleted file mode 100644 index fad994f..0000000 --- a/debian/ros-ROS_DISTRO-mc-log-visualization.install +++ /dev/null @@ -1,3 +0,0 @@ -opt/ros/@ROS_DISTRO@/lib/mc_log_visualization/* -opt/ros/@ROS_DISTRO@/lib/pkgconfig/mc_log_visualization.pc -opt/ros/@ROS_DISTRO@/share/mc_log_visualization/* diff --git a/debian/ros-ROS_DISTRO-mc-surfaces-visualization.install b/debian/ros-ROS_DISTRO-mc-surfaces-visualization.install deleted file mode 100644 index 6090e78..0000000 --- a/debian/ros-ROS_DISTRO-mc-surfaces-visualization.install +++ /dev/null @@ -1,3 +0,0 @@ -opt/ros/@ROS_DISTRO@/lib/mc_surfaces_visualization/* -opt/ros/@ROS_DISTRO@/lib/pkgconfig/mc_surfaces_visualization.pc -opt/ros/@ROS_DISTRO@/share/mc_surfaces_visualization/* diff --git a/mc_convex_visualization/.gitmodules b/mc_convex_visualization/.gitmodules deleted file mode 100644 index 2b7a4fe..0000000 --- a/mc_convex_visualization/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "cmake"] - path = cmake - url = https://github.com/jrl-umi3218/jrl-cmakemodules.git diff --git a/mc_convex_visualization/CMakeLists.txt b/mc_convex_visualization/CMakeLists.txt deleted file mode 100644 index 4c51631..0000000 --- a/mc_convex_visualization/CMakeLists.txt +++ /dev/null @@ -1,33 +0,0 @@ -# -# Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL -# - -cmake_minimum_required(VERSION 3.1) -set(CMAKE_CXX_STANDARD 17) -set(PROJECT_NAME mc_convex_visualization) -project( - ${PROJECT_NAME} - LANGUAGES CXX - VERSION 1.5.0) - -find_package(catkin REQUIRED COMPONENTS visualization_msgs roscpp std_msgs) - -catkin_package(CATKIN_DEPENDS roscpp) - -find_package(mc_rtc REQUIRED) - -include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) - -set(src_files src/main.cpp) - -add_executable(sch_visualizer ${src_files}) -target_link_libraries(sch_visualizer PUBLIC mc_rtc::mc_rbdyn mc_rtc::mc_rtc_ros - ${catkin_LIBRARIES}) - -install( - TARGETS sch_visualizer - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/mc_convex_visualization/README.md b/mc_convex_visualization/README.md deleted file mode 100644 index 389d8c5..0000000 --- a/mc_convex_visualization/README.md +++ /dev/null @@ -1,24 +0,0 @@ -# mc\_convex\_visualization - -This tool allows to visualize the convexes of an mc\_rtc robot in the RViZ application. - -## Launching the visualization - -The visualization is launched as follows: - -```bash -$ roslaunch mc_convex_visualization display.launch robot:=JVRC1 -``` - -The `robot` argument should be the same as what you would pass to the `mc_rbdyn::RobotLoader::get_robot_module` function, this can also be a robot alias. For example: - -```bash -# Specify a vector of arguments -$ roslaunch mc_convex_visualization display.launch robot:="[env, `rospack find mc_env_description`, ground]" -# Or an alias -$ roslaunch mc_convex_visualization display.launch robot:=env/ground -``` - -## Convexes visualization - -You can show/hide selected convexes by enabling/disabling their namespaces in the associated MarkerArray diff --git a/mc_convex_visualization/launch/convex.rviz b/mc_convex_visualization/launch/convex.rviz deleted file mode 100644 index df3ebe7..0000000 --- a/mc_convex_visualization/launch/convex.rviz +++ /dev/null @@ -1,150 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Grid1 - - /RobotModel1 - Splitter Ratio: 0.5 - Tree Height: 737 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 0.5 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - door: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - handle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /sch_marker - Name: MarkerArray - Namespaces: - door: true - handle: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: robot_map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 1.75763 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.118602 - Y: 0.717184 - Z: 0.779032 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.390398 - Target Frame: - Value: Orbit (rviz) - Yaw: 5.39358 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1051 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000377fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000377000000c200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000377000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000040fc0100000002fb0000000800540069006d00650100000000000007800000031600fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000037700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 1680 - Y: 0 diff --git a/mc_convex_visualization/launch/convex_publisher.launch b/mc_convex_visualization/launch/convex_publisher.launch deleted file mode 100644 index 2367bc4..0000000 --- a/mc_convex_visualization/launch/convex_publisher.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/mc_convex_visualization/launch/display.launch b/mc_convex_visualization/launch/display.launch deleted file mode 100644 index 646856d..0000000 --- a/mc_convex_visualization/launch/display.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/mc_convex_visualization/package.xml b/mc_convex_visualization/package.xml deleted file mode 100644 index 8d2cc11..0000000 --- a/mc_convex_visualization/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - mc_convex_visualization - 1.5.0 - Visualize hulls in RVIz - - Hervé Audren - Pierre Gergondet - - BSD - - catkin - - roscpp - std_msgs - visualization_msgs - - roscpp - std_msgs - visualization_msgs - - - - diff --git a/mc_convex_visualization/src/main.cpp b/mc_convex_visualization/src/main.cpp deleted file mode 100644 index 5f7f3bf..0000000 --- a/mc_convex_visualization/src/main.cpp +++ /dev/null @@ -1,288 +0,0 @@ -/* - * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL - */ - -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include - -namespace -{ -template -void getParam(ros::NodeHandle & n, const std::string & param, T & out) -{ - std::string true_param; - n.searchParam(param, true_param); - n.getParam(true_param, out); -} - -std::vector robotParam(ros::NodeHandle & n) -{ - std::string robot_str = ""; - getParam(n, "robot", robot_str); - if(robot_str.size() == 0) { return {}; } - if(robot_str[0] == '[') - { - std::vector ret = {""}; - for(size_t i = 1; i < robot_str.size(); ++i) - { - const auto & c = robot_str[i]; - if(c == ',') { ret.push_back(""); } - else if(c != ']' && c != ' ') { ret.back() += c; } - } - return ret; - } - return {robot_str}; -} - -visualization_msgs::Marker initMarker(const std::string & frame_id, const std::string & name, size_t id, int32_t t) -{ - visualization_msgs::Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = ros::Time(); - marker.ns = name; - marker.id = static_cast(id); - marker.type = t; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = 0; - marker.pose.position.y = 0; - marker.pose.position.z = 0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 1; - marker.scale.y = 1.0; - marker.scale.z = 1.0; - marker.color.a = 1.0; // Don't forget to set the alpha! - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - marker.lifetime = ros::Duration(0.5); - return marker; -} - -void setMarkerPose(visualization_msgs::Marker & marker, const sva::PTransformd & pt) -{ - auto q = Eigen::Quaterniond(pt.rotation().transpose()); - const auto & t = pt.translation(); - marker.pose.orientation.w = q.w(); - marker.pose.orientation.x = q.x(); - marker.pose.orientation.y = q.y(); - marker.pose.orientation.z = q.z(); - marker.pose.position.x = t.x(); - marker.pose.position.y = t.y(); - marker.pose.position.z = t.z(); -} - -visualization_msgs::Marker fromPolyhedron(const std::string & frame_id, - const std::string & name, - size_t id, - sch::S_Polyhedron & poly, - const sva::PTransformd & colTrans) -{ - auto marker = initMarker(frame_id, name, id, visualization_msgs::Marker::TRIANGLE_LIST); - auto & pa = *poly.getPolyhedronAlgorithm(); - const auto triangles = pa.triangles_; - const auto vertexes = pa.vertexes_; - for(unsigned int i = 0; i < triangles.size(); i++) - { - const auto a = vertexes[triangles[i].a]->getCoordinates(); - const auto b = vertexes[triangles[i].b]->getCoordinates(); - const auto c = vertexes[triangles[i].c]->getCoordinates(); - sva::PTransformd va(Eigen::Vector3d(a[0], a[1], a[2])); - sva::PTransformd vb(Eigen::Vector3d(b[0], b[1], b[2])); - sva::PTransformd vc(Eigen::Vector3d(c[0], c[1], c[2])); - const auto normal = triangles[i].normal; - auto cross = (a - b) ^ (a - c); - auto dot = normal * (cross / cross.norm()); - bool reversePointOrder = dot < 0; - std::vector vertexOrder = {va, vb, vc}; - if(reversePointOrder) { vertexOrder = {vc, vb, va}; } - for(size_t j = 0; j < vertexOrder.size(); j++) - { - vertexOrder[j] = vertexOrder[j] * colTrans; - geometry_msgs::Point p; - p.x = vertexOrder[j].translation().x(); - p.y = vertexOrder[j].translation().y(); - p.z = vertexOrder[j].translation().z(); - marker.points.push_back(p); - } - } - return marker; -} - -visualization_msgs::Marker fromBox(const std::string & frame_id, - const std::string & name, - size_t id, - sch::S_Box & box, - const sva::PTransformd & colTrans) -{ - auto marker = initMarker(frame_id, name, id, visualization_msgs::Marker::CUBE); - auto & scale = marker.scale; - box.getBoxParameters(scale.x, scale.y, scale.z); - setMarkerPose(marker, colTrans); - return marker; -} - -visualization_msgs::Marker fromCylinder(const std::string & frame_id, - const std::string & name, - size_t id, - sch::S_Cylinder & cylinder, - const sva::PTransformd & colTrans) -{ - auto marker = initMarker(frame_id, name, id, visualization_msgs::Marker::CYLINDER); - marker.scale.x = 2 * cylinder.getRadius(); - marker.scale.y = 2 * cylinder.getRadius(); - marker.scale.z = (cylinder.getP2() - cylinder.getP1()).norm(); - auto midP = cylinder.getP1() + (cylinder.getP2() - cylinder.getP1()) / 2; - Eigen::Vector3d midPV3{midP.m_x, midP.m_y, midP.m_z}; - sva::PTransformd cylinderCenter{Eigen::Matrix3d::Identity(), midPV3}; - setMarkerPose(marker, colTrans * cylinderCenter); - return marker; -} - -visualization_msgs::Marker fromSphere(const std::string & frame_id, - const std::string & name, - size_t id, - sch::S_Sphere & sphere, - const sva::PTransformd & colTrans) -{ - auto marker = initMarker(frame_id, name, id, visualization_msgs::Marker::SPHERE); - marker.scale.x = 2 * sphere.getRadius(); - marker.scale.y = 2 * sphere.getRadius(); - marker.scale.z = 2 * sphere.getRadius(); - setMarkerPose(marker, colTrans); - return marker; -} - -visualization_msgs::MarkerArray convexMarkers(const std::string & tf_prefix, - const mc_rbdyn::Robot & robot, - const std::vector & filtered_convexes) -{ - visualization_msgs::MarkerArray markers; - unsigned id = 0; - for(const auto & col : robot.convexes()) - { - if(std::find(filtered_convexes.begin(), filtered_convexes.end(), col.first) != filtered_convexes.end()) - { - continue; - } - const auto & frame = col.second.first; - sch::S_Object * object = col.second.second.get(); - const auto & colTrans = robot.collisionTransform(col.first); - if(sch::S_Polyhedron * poly = dynamic_cast(object)) - { - markers.markers.push_back(fromPolyhedron(tf_prefix + frame, col.first, ++id, *poly, colTrans)); - } - else if(sch::S_Box * box = dynamic_cast(object)) - { - markers.markers.push_back(fromBox(tf_prefix + frame, col.first, ++id, *box, colTrans)); - } - else if(sch::S_Cylinder * cylinder = dynamic_cast(object)) - { - markers.markers.push_back(fromCylinder(tf_prefix + frame, col.first, ++id, *cylinder, colTrans)); - } - else if(sch::S_Sphere * sphere = dynamic_cast(object)) - { - markers.markers.push_back(fromSphere(tf_prefix + frame, col.first, ++id, *sphere, colTrans)); - } - else { mc_rtc::log::warning("Cannot display {} collision object", col.first); } - } - return markers; -} - -mc_rbdyn::RobotModulePtr rmFromParam(const std::vector & robot_params) -{ - if(robot_params.size() == 1) { return mc_rbdyn::RobotLoader::get_robot_module(robot_params[0]); } - else if(robot_params.size() == 2) - { - return mc_rbdyn::RobotLoader::get_robot_module(robot_params[0], robot_params[1]); - } - else if(robot_params.size() == 3) - { - return mc_rbdyn::RobotLoader::get_robot_module(robot_params[0], robot_params[1], robot_params[2]); - } - else if(robot_params.size() > 3) - { - ROS_ERROR_STREAM("Invalid robot_params size passed to mc_surfaces_visualization: " << robot_params.size()); - } - return nullptr; -} - -} // namespace - -int main(int argc, char ** argv) -{ - ros::init(argc, argv, "mc_convex_visualization"); - - ros::NodeHandle n; - ros::NodeHandle n_private("~"); - - std::vector robot_module = {}; - getParam(n, "robot_module", robot_module); - std::vector robot_param = robotParam(n_private); - bool robot_set = robot_param.size() != 0; - std::vector robot_params = robot_set ? robot_param : robot_module; - - std::vector filtered_convexes = {}; - getParam(n, "filtered_convexes", filtered_convexes); - - std::string tf_prefix = ""; - getParam(n, "tf_prefix", tf_prefix); - if(tf_prefix.size() && tf_prefix[tf_prefix.size() - 1] != '/') { tf_prefix += '/'; } - - bool publish = false; - getParam(n_private, "publish", publish); - - ros::Publisher sch_pub = n.advertise("sch_marker", 1000); - - std::shared_ptr robotModule = nullptr; - std::unique_ptr robot_pub; - std::shared_ptr robots; - visualization_msgs::MarkerArray markers; - auto init = [&]() - { - robotModule = rmFromParam(robot_params); - if(!robotModule) { return; } - robots = mc_rbdyn::loadRobot(*robotModule); - markers = convexMarkers(tf_prefix, robots->robot(), filtered_convexes); - if(publish) - { - robot_pub.reset(new mc_rtc::RobotPublisher(tf_prefix, 50, 0.01)); - robot_pub->init(robots->robot()); - } - }; - - init(); - - ros::Rate rate(10); - while(ros::ok()) - { - sch_pub.publish(markers); - if(robot_pub) { robot_pub->update(0.01, robots->robot()); } - getParam(n, "robot_module", robot_module); - if(!robot_set && robot_module.size() && robot_module != robot_params) - { - robot_params = robot_module; - init(); - } - ros::spinOnce(); - rate.sleep(); - } - - return 0; -} diff --git a/mc_log_visualization/CMakeLists.txt b/mc_log_visualization/CMakeLists.txt deleted file mode 100644 index 7219576..0000000 --- a/mc_log_visualization/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -# -# Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL -# - -cmake_minimum_required(VERSION 3.1) -set(CMAKE_CXX_STANDARD 17) -set(PROJECT_NAME mc_log_visualization) -project( - ${PROJECT_NAME} - LANGUAGES CXX - VERSION 1.5.0) - -find_package(catkin REQUIRED COMPONENTS visualization_msgs roscpp std_msgs) - -catkin_package() - -find_package(mc_rtc REQUIRED) - -include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) - -set(src_files src/mc_log_visualization.cpp src/LogPublisher.h - src/LogPublisher.cpp src/LogRobot.h src/LogRobot.cpp) - -add_executable(mc_log_visualization ${src_files}) -target_link_libraries( - mc_log_visualization PUBLIC ${catkin_LIBRARIES} mc_rtc::mc_control - mc_rtc::mc_rtc_ros) - -install( - TARGETS mc_log_visualization - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/mc_log_visualization/README.md b/mc_log_visualization/README.md deleted file mode 100644 index 5e14bc4..0000000 --- a/mc_log_visualization/README.md +++ /dev/null @@ -1,33 +0,0 @@ -# mc\_log\_visualization - -This tool allows you to visualize a log in 3D in the RViZ application. - -## Launching the visualization - -The replay is launched as follows: - -```bash -$ roslaunch mc_log_visualization log_visualizer.launch robot:=JVRC1 log:=/tmp/mc-control-MyController-latest.bin -``` - -_Note: you *must* give a full path to the log here due to ROS limitations_ - -You then launch the visualization: - -```bash -$ roslaunch mc_rtc_ticker display.launch -``` - -## Time range selector - -This selector lets you select a time range for the visualization. Currently you should have saved this range in your log. - -## Add extra information - -This menu lets you select extra information to display in the interface, based on the type of data was logged the tool will choose how to display the data. - -In some cases, the UI will let you specify extra information before displaying the data. For example, in the screenshot, the `RightFootForceSensor` entry is selected so the UI will ask you in which surface frame the force should be displayed. - -After adding extra information, it will be displayed in the 3D environment if applicable and the numeric data will be displayed in the `Log visualizer - Extra data` tab. In this tab, you can also remove the data from the visualization. - -Extra data selection is saved across runs of the log visualizer. diff --git a/mc_log_visualization/launch/log_visualizer.launch b/mc_log_visualization/launch/log_visualizer.launch deleted file mode 100644 index c705827..0000000 --- a/mc_log_visualization/launch/log_visualizer.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/mc_log_visualization/package.xml b/mc_log_visualization/package.xml deleted file mode 100644 index 2dc896f..0000000 --- a/mc_log_visualization/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - mc_log_visualization - 1.5.0 - Visualize logs obtained from mc_rtc in RViz - - Pierre Gergondet - - BSD - - catkin - - roscpp - std_msgs - visualization_msgs - mc_rtc - mc_rtc_msgs - - roscpp - std_msgs - visualization_msgs - mc_rtc - mc_rtc_msgs - diff --git a/mc_log_visualization/src/LogPublisher.cpp b/mc_log_visualization/src/LogPublisher.cpp deleted file mode 100644 index 279612a..0000000 --- a/mc_log_visualization/src/LogPublisher.cpp +++ /dev/null @@ -1,485 +0,0 @@ -/* - * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL - */ - -#include "LogPublisher.h" - -LogPublisher::LogPublisher(ros::NodeHandle & nh_, - const std::string & logfile, - mc_rbdyn::RobotModulePtr mod_, - double dt_) -: nh(nh_), mod(mod_), dt(dt_), rate(1 / dt), rt(rate) -{ - log.load(logfile); - if(log.has("t")) - { - const auto & t = log.getRaw("t"); - if(t.size() > 2 && t[0] && t[1]) - { - dt = *t[1] - *t[0]; - rate = 1 / dt; - rt = rate; - mc_rtc::log::info("[LogPublisher] Log timestep: {}", dt); - } - } - LogRobot::Configuration conf; - conf.rm = mod; - conf.dt = dt; - conf.encoders = "qIn"; - { - conf.id = "control"; - conf.configuration = "qOut"; - conf.base = "ff"; - robot.reset(new LogRobot(conf)); - } - { - conf.id = "real"; - conf.configuration = "qIn"; - if(log.has("realRobot_posW")) - { - conf.base = "realRobot_posW"; - real_robot.reset(new LogRobot(conf)); - } - else if(log.has("rpyIn")) - { - conf.base = "ff"; - conf.base_rotation = "rpyIn"; - conf.base_rotation_is_imu = true; - real_robot.reset(new LogRobot(conf)); - } - else { real_robot.reset(nullptr); } - } -} - -void LogPublisher::pubThread() -{ - unsigned int pub_i = 0; - - rt = ros::Rate(rate); - while(running) - { - robot->update(log, cur_i); - if(real_robot) { real_robot->update(log, cur_i); } - - /* Playback speed logic */ - pub_i++; - if(!paused && pub_i % playback_den == 0) - { - if(!reversed) { cur_i += playback_num; } - else { cur_i -= playback_num; } - pub_i = 0; - } - if(cur_i >= max_i) - { - if(reversed) { cur_i = max_i - 1; } - else { cur_i = min_i; } - } - if(cur_i < min_i) { cur_i = max_i; } - cur_t = log.get("t", cur_i, cur_t); - server->handle_requests(gui); - server->publish(gui); - rt.sleep(); - } -} - -void LogPublisher::addRemoveExtraDataButton(const std::string & section, const std::string & entry) -{ - gui.addElement(extraDataRemoveCategory, mc_rtc::gui::Button("Remove " + entry, - [this, section, entry]() - { - gui.removeElement(extraDataCategory, entry); - gui.removeElement(extraDataRemoveCategory, - "Remove " + entry); - config(section).remove(entry); - saveConfig(); - })); -} - -void LogPublisher::rebuildGUI() -{ - std::vector category = {"Log visualizer"}; - gui.removeCategory(category); - - std::vector robots_category = {"Log visualizer - Robots"}; - gui.removeCategory(robots_category); - gui.addElement(robots_category, mc_rtc::gui::Robot(robot->robot().name(), - [this]() -> const mc_rbdyn::Robot & { return robot->robot(); })); - robots_category.push_back("Real"); - if(real_robot) - { - gui.addElement(robots_category, - mc_rtc::gui::Robot(real_robot->robot().name(), - [this]() -> const mc_rbdyn::Robot & { return real_robot->robot(); })); - } - - auto makeTimeElement = [this]() - { - return mc_rtc::gui::NumberSlider( - "#Time", [this]() { return cur_t; }, - [this](double time) - { - size_t i = min_i; - double t_i = log.get("t", i, 0); - double t_ii = log.get("t", i + 1, 0); - while(t_ii < time && i < max_i - 1) - { - i++; - t_i = t_ii; - t_ii = log.get("t", i + 1, t_i); - } - cur_i = i; - cur_t = t_i; - }, - min_t, max_t); - }; - gui.addElement(category, mc_rtc::gui::ElementsStacking::Horizontal, - mc_rtc::gui::Checkbox( - "Paused", [this]() { return paused; }, [this]() { paused = !paused; }), - makeTimeElement(), - mc_rtc::gui::Checkbox( - "Reverse", [this]() { return reversed; }, [this]() { reversed = !reversed; })); - - gui.addElement(category, mc_rtc::gui::ElementsStacking::Horizontal, - mc_rtc::gui::Button("-", - [this]() - { - if(playback_num == 1) { playback_den *= 2; } - else { playback_num /= 2; } - }), - mc_rtc::gui::Label("Playback speed:", - [this]() - { - std::stringstream ss; - ss << playback_num; - if(playback_den != 1) { ss << "/" << playback_den; } - return ss.str(); - }), - mc_rtc::gui::Button("+", - [this]() - { - if(playback_den == 1) { playback_num *= 2; } - else { playback_den /= 2; } - })); - - auto makeStepButton = [this](int mul) - { - int dt_ms = static_cast(std::floor(mul * 1000 * dt)); - std::stringstream ss; - if(dt_ms > 0) { ss << "+"; } - ss << dt_ms << " ms"; - return mc_rtc::gui::Button( - ss.str(), - [this, mul]() - { - paused = true; - cur_i = static_cast( - std::max(static_cast(min_i), std::min(static_cast(max_i), static_cast(cur_i) + mul))); - }); - }; - gui.addElement(category, mc_rtc::gui::ElementsStacking::Horizontal, makeStepButton(-100), makeStepButton(-10), - makeStepButton(-1), makeStepButton(1), makeStepButton(10), makeStepButton(100)); - - std::vector keys; - for(const auto & k : log.entries()) - { - if(k == "t" || (k.size() > 1 && k[0] == 't' && k[1] == '_')) { keys.push_back(k); } - } - gui.addElement( - category, mc_rtc::gui::ComboInput( - "Time range", keys, [this]() { return t_data; }, [this](const std::string & t) { selectTime(t); })); - gui.addElement(category, mc_rtc::gui::Button("Stop log replay", [this]() { running = false; })); - - category.push_back("Add extra information"); - const auto & log_entries = log.entries(); - std::vector entries{log_entries.begin(), log_entries.end()}; - if(!log.has(extra_selected)) { extra_selected = entries[0]; } - gui.addElement(category, mc_rtc::gui::ComboInput( - "Entry", entries, [this]() { return extra_selected; }, - [this](const std::string & selected) - { - extra_selected = selected; - rebuildGUI(); - })); - std::string entry = extra_selected; - auto types = log.types(entry); - bool added_something = false; - for(const auto & t : types) - { - switch(t) - { - case mc_rtc::log::LogType::Bool: - case mc_rtc::log::LogType::Int8_t: - case mc_rtc::log::LogType::Int16_t: - case mc_rtc::log::LogType::Int32_t: - case mc_rtc::log::LogType::Int64_t: - case mc_rtc::log::LogType::Uint8_t: - case mc_rtc::log::LogType::Uint16_t: - case mc_rtc::log::LogType::Uint32_t: - case mc_rtc::log::LogType::Uint64_t: - case mc_rtc::log::LogType::Float: - case mc_rtc::log::LogType::Double: - case mc_rtc::log::LogType::String: - case mc_rtc::log::LogType::Quaterniond: - case mc_rtc::log::LogType::Vector2d: - case mc_rtc::log::LogType::Vector6d: - case mc_rtc::log::LogType::VectorXd: - case mc_rtc::log::LogType::MotionVecd: - case mc_rtc::log::LogType::VectorDouble: - added_something = true; - gui.addElement(category, mc_rtc::gui::Button("Add " + entry + " as Label", - [this, entry, t]() - { - config("labels").add(entry, static_cast(t)); - saveConfig(); - addLabel(entry, t); - })); - break; - case mc_rtc::log::LogType::Vector3d: - added_something = true; - gui.addElement(category, mc_rtc::gui::Button("Add " + entry + " as Point3D", - [this, entry]() - { - config("points").add(entry, "point"); - saveConfig(); - addVector3dAsPoint3D(entry); - })); - break; - case mc_rtc::log::LogType::ForceVecd: - added_something = true; - gui.addElement(category, mc_rtc::gui::Form( - "Add " + entry + " as Force", - [this, entry](const mc_rtc::Configuration & form) - { - std::string surface = form("Surface"); - auto c = config("forces").add(entry); - c.add("surface", surface); - saveConfig(); - addForce(entry, surface); - }, - mc_rtc::gui::FormComboInput("Surface", true, robot->surfaces()))); - break; - case mc_rtc::log::LogType::PTransformd: - added_something = true; - gui.addElement(category, mc_rtc::gui::Button("Add " + entry + " as Transform", - [this, entry]() - { - config("transforms").add(entry, "ptransformd"); - saveConfig(); - addPTransformdAsTransform(entry); - })); - break; - case mc_rtc::log::LogType::None: - continue; - }; - } - if(!added_something) - { - gui.addElement(category, mc_rtc::gui::Label("Cannot display " + entry, []() { return ""; })); - } -} - -bool LogPublisher::selectTime(const std::string & t) -{ - size_t i = 0; - double t_i = 0; - double t_ii = 0; - do { - t_i = log.get(t, i, 0); - t_ii = log.get(t, i + 1, 0); - } while(t_i == t_ii && ++i < log.size() - 1); - if(i + 1 == log.size()) { return false; } - if(fabs(t_ii - t_i - dt) > 1e-6) { i = i + 1; } - size_t start_i = i; - do { - t_i = log.get(t, i, 0); - t_ii = log.get(t, i + 1, 0); - } while(fabs(t_ii - t_i - dt) < 1e-6 && ++i < log.size() - 1); - if(i == start_i) { return false; } - min_i = start_i; - cur_i = min_i; - max_i = i; - min_t = log.get(t, min_i, 0); - cur_t = min_t; - max_t = log.get(t, max_i, 0); - t_data = t; - rebuildGUI(); - return true; -} - -void LogPublisher::run() -{ - selectTime("t"); - - server.reset( - new mc_control::ControllerServer(dt, 10 * dt, {"ipc:///tmp/mc_rtc_pub.ipc"}, {"ipc:///tmp/mc_rtc_rep.ipc"})); - - pub_th = std::thread(std::bind(&LogPublisher::pubThread, this)); - rebuildGUI(); - loadConfig(); - pub_th.join(); -} - -namespace details -{ - -template -void addLabel(LogPublisher & log, const std::string & entry, const T & def) -{ - log.gui.addElement( - log.extraDataCategory, - mc_rtc::gui::Label(entry, [&log, entry, def]() -> RetT { return log.log.get(entry, log.cur_i, def); })); - log.addRemoveExtraDataButton("labels", entry); -} - -template -void addLabel(LogPublisher & log, const std::string & entry, const T & def, const std::vector & labels) -{ - log.gui.addElement(log.extraDataCategory, mc_rtc::gui::ArrayLabel(entry, labels, - [&log, entry, def]() -> const T & - { - auto data_ptr = - log.log.getRaw(entry, log.cur_i); - return data_ptr ? *data_ptr : def; - })); - log.addRemoveExtraDataButton("labels", entry); -} -} // namespace details - -void LogPublisher::addLabel(const std::string & entry, mc_rtc::log::LogType t) -{ - switch(t) - { - case mc_rtc::log::LogType::Bool: - details::addLabel(*this, entry, true); - break; - case mc_rtc::log::LogType::Int8_t: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Int16_t: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Int32_t: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Int64_t: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Uint8_t: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Uint16_t: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Uint32_t: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Uint64_t: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Float: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::Double: - details::addLabel(*this, entry, 0); - break; - case mc_rtc::log::LogType::String: - details::addLabel(*this, entry, ""); - break; - case mc_rtc::log::LogType::Quaterniond: - details::addLabel(*this, entry, Eigen::Quaterniond::Identity(), {"w", "x", "y", "z"}); - break; - case mc_rtc::log::LogType::Vector2d: - details::addLabel(*this, entry, Eigen::Vector2d::Zero(), {"x", "y"}); - break; - case mc_rtc::log::LogType::Vector6d: - details::addLabel(*this, entry, {}, {}); - break; - case mc_rtc::log::LogType::VectorXd: - details::addLabel(*this, entry, {}, {}); - break; - case mc_rtc::log::LogType::MotionVecd: - details::addLabel(*this, entry, sva::MotionVecd::Zero(), {"wx", "wy", "wz", "vx", "vy", "vz"}); - break; - case mc_rtc::log::LogType::VectorDouble: - details::addLabel>(*this, entry, {}, {}); - break; - default: - break; - } -} - -void LogPublisher::addVector3dAsPoint3D(const std::string & entry) -{ - gui.addElement(extraDataCategory, - mc_rtc::gui::Point3D(entry, [this, entry]() - { return log.get(entry, cur_i, Eigen::Vector3d::Zero()); })); - addRemoveExtraDataButton("points", entry); -} - -void LogPublisher::addForce(const std::string & entry, const std::string & surface) -{ - gui.addElement(extraDataCategory, - mc_rtc::gui::Force( - entry, [this, entry]() { return log.get(entry, cur_i, sva::ForceVecd::Zero()); }, - [this, surface]() { return robot->robot().surfacePose(surface); })); - addRemoveExtraDataButton("forces", entry); -} - -void LogPublisher::addPTransformdAsTransform(const std::string & entry) -{ - gui.addElement( - extraDataCategory, - mc_rtc::gui::Transform(entry, [this, entry]() - { return log.get(entry, cur_i, sva::PTransformd::Identity()); })); - addRemoveExtraDataButton("transforms", entry); -} - -bfs::path LogPublisher::configPath() const -{ -#ifndef WIN32 - return bfs::path(std::getenv("HOME")) / ".config/mc_rtc/log_visualizer.conf"; -#else - // Should work for Windows Vista and up - retrn config_path = bfs::path(std::getenv("APPDATA")) / "mc_rtc/log_visualizer.conf"; -#endif -} - -void LogPublisher::loadConfig() -{ - auto p = configPath(); - if(bfs::exists(p)) { config.load(p.string()); } - std::vector requiredKeys = {"labels", "forces", "points", "transforms"}; - for(const auto & k : requiredKeys) - { - if(!config.has(k)) { config.add(k); } - } - for(const auto & e : config("labels").keys()) - { - if(log.has(e)) - { - int type = config("labels")(e); - addLabel(e, mc_rtc::log::LogType(type)); - } - } - for(const auto & pt : config("points").keys()) - { - if(log.has(pt)) { addVector3dAsPoint3D(pt); } - } - for(const auto & f : config("forces").keys()) - { - if(log.has(f)) - { - std::string surface = config("forces")(f)("surface"); - addForce(f, surface); - } - } - for(const auto & t : config("transforms").keys()) - { - if(log.has(t)) { addPTransformdAsTransform(t); } - } -} - -void LogPublisher::saveConfig() const -{ - config.save(configPath().string()); -} diff --git a/mc_log_visualization/src/LogPublisher.h b/mc_log_visualization/src/LogPublisher.h deleted file mode 100644 index ebab391..0000000 --- a/mc_log_visualization/src/LogPublisher.h +++ /dev/null @@ -1,119 +0,0 @@ -/* - * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL - */ - -#pragma once - -#include "LogRobot.h" - -#include -#include -#include -#include -#include -#include - -#include - -#include -namespace bfs = boost::filesystem; - -#include -#include -#include - -/** Publish an mc_rtc binary log using the GUI facilities */ -struct LogPublisher -{ -public: - /** Constructor */ - LogPublisher(ros::NodeHandle & nh, const std::string & logfile, mc_rbdyn::RobotModulePtr mod, double dt); - - /** Start the publication thread */ - void pubThread(); - - /** Add a button to the GUI to remove a log entry that has been added to the GUI - * - * \param section Section of the configuration where the data is stored - * - * \param entry Entry in the log - */ - void addRemoveExtraDataButton(const std::string & section, const std::string & entry); - - /** Add a given entry as a label */ - void addLabel(const std::string & entry, mc_rtc::log::LogType t); - - /** Add a 3D point from a Vector3d log entry to the GUI */ - void addVector3dAsPoint3D(const std::string & entry); - - /** Add a Transform from a PTransformd entry to the GUI */ - void addPTransformdAsTransform(const std::string & entry); - - /** Add a force recording from the log to the GUI */ - void addForce(const std::string & entry, const std::string & surface); - - /** Get the filesystem path to the configuration */ - bfs::path configPath() const; - - /** Load the configuration and restore GUI elements */ - void loadConfig(); - - /** Save the configuration to disk */ - void saveConfig() const; - - /** Update the GUI */ - void rebuildGUI(); - - /** Select a time range for the publication */ - bool selectTime(const std::string & t); - - /** Run the main application */ - void run(); - -public: - /** Used to store/load the extra data provided by the log */ - mc_rtc::Configuration config; - /* ROS */ - ros::NodeHandle & nh; - std::shared_ptr mod; - - bool running = true; - std::thread pub_th; - - /** Log data */ - mc_rtc::log::FlatLog log; - - /** Time parameters */ - double dt; - double rate; - ros::Rate rt; - - /** Play/pause playback */ - bool paused = false; - /** Play in reverse */ - bool reversed = false; - /** Playback speed numerator */ - unsigned int playback_num = 1; - /** Playback speed denominator */ - unsigned int playback_den = 1; - /* min/max/current playback index/time */ - double min_t = 0; - size_t min_i = 0; - double max_t = 0; - size_t max_i = 0; - double cur_t = 0; - size_t cur_i = 0; - /* Current time data */ - std::string t_data; - /* Current extra entry menu currently selected */ - std::string extra_selected = ""; - - std::unique_ptr robot; - std::unique_ptr real_robot; - - /* UI related */ - mc_rtc::gui::StateBuilder gui; - std::unique_ptr server; - const std::vector extraDataCategory = {"Log visualizer - Extra data"}; - const std::vector extraDataRemoveCategory = {"Log visualizer - Extra data", "Remove"}; -}; diff --git a/mc_log_visualization/src/LogRobot.cpp b/mc_log_visualization/src/LogRobot.cpp deleted file mode 100644 index 81acb86..0000000 --- a/mc_log_visualization/src/LogRobot.cpp +++ /dev/null @@ -1,62 +0,0 @@ -/* - * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL - */ - -#include "LogRobot.h" - -#include - -#include - -LogRobot::LogRobot(const LogRobot::Configuration & config) -: config_(config), publisher_(config.id + "/", 1 / config.dt, config.dt), robots_(mc_rbdyn::loadRobot(*config.rm)) -{ -} - -void LogRobot::update(const mc_rtc::log::FlatLog & log, size_t i) -{ - q = log.get(config_.configuration, i, q); - encoders = log.get(config_.encoders, i, encoders); - if(config_.base.size()) { base = log.get(config_.base, i, base); } - if(config_.base_translation.size()) { base.translation() = log.get(config_.base_translation, i, base.translation()); } - if(config_.base_rotation.size()) - { - if(config_.base_rotation_is_imu) - { - if(i == 0) - { - X_imu0_0 = {log.get(config_.base_rotation, i, {1, 0, 0, 0})}; - X_imu0_0 = X_imu0_0.inv(); - } - sva::PTransformd X_0_imu = {log.get(config_.base_rotation, i, {1, 0, 0, 0})}; - base.rotation() = (X_0_imu * X_imu0_0).rotation(); - } - else { base.rotation() = Eigen::Matrix3d(log.get(config_.base_rotation, i, Eigen::Quaterniond(base.rotation()))); } - } - auto & robot = robots_->robot(); - for(size_t refIdx = 0; refIdx < robot.refJointOrder().size(); ++refIdx) - { - const auto & jN = robot.refJointOrder()[refIdx]; - if(robot.hasJoint(jN)) - { - auto jIndex = robot.jointIndexByName(jN); - if(robot.mbc().q[jIndex].size() == 1) { robot.mbc().q[jIndex][0] = q[refIdx]; } - } - } - robot.posW(base); - publisher_.update(config_.dt, robot); -} - -std::vector LogRobot::surfaces() const -{ - std::vector ret; - for(const auto & p : robot().surfaces()) { ret.push_back(p.first); } - return ret; -} - -std::vector LogRobot::bodies() const -{ - std::vector ret; - for(const auto & b : robot().mb().bodies()) { ret.push_back(b.name()); } - return ret; -} diff --git a/mc_log_visualization/src/LogRobot.h b/mc_log_visualization/src/LogRobot.h deleted file mode 100644 index cc9b1bd..0000000 --- a/mc_log_visualization/src/LogRobot.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL - */ - -#pragma once - -#include -#include -#include -#include - -/** Publish a robot state based on log data*/ -struct LogRobot -{ - struct Configuration - { - /** RobotModule used for robots initialization */ - mc_rbdyn::RobotModulePtr rm; - /** Timestep of the log */ - double dt; - /** Identifier and TF prefix */ - std::string id; - /** Configuration entry in the log */ - std::string configuration; - /** Encoders entry in the log */ - std::string encoders; - /** PTransfomd entry used for floating-base estimation */ - std::string base; - /** If not empty, use this as floating-base rotation */ - std::string base_rotation; - /** If not empty, use this as floating-base translation */ - std::string base_translation; - /** If true, interpret base_rotation as an IMU reading */ - bool base_rotation_is_imu = false; - }; - - /** Create a LogRobot with the provided configuration */ - LogRobot(const Configuration & config); - - /** Update the published robot state */ - void update(const mc_rtc::log::FlatLog & log, size_t i); - - /** Access underlying robot */ - inline const mc_rbdyn::Robot & robot() const { return robots_->robot(); } - - /** Access all surfaces in the robot */ - std::vector surfaces() const; - - /** Access all bodies in the robot */ - std::vector bodies() const; - -private: - Configuration config_; - mc_rtc::RobotPublisher publisher_; - std::shared_ptr robots_; - sva::PTransformd base; - std::vector q; - std::vector encoders; - // Inverse of initial IMU orientation - sva::PTransformd X_imu0_0; -}; diff --git a/mc_log_visualization/src/mc_log_visualization.cpp b/mc_log_visualization/src/mc_log_visualization.cpp deleted file mode 100644 index 9aeddf7..0000000 --- a/mc_log_visualization/src/mc_log_visualization.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL - */ - -#include "LogPublisher.h" - -namespace -{ -template -void getParam(ros::NodeHandle & n, const std::string & param, T & out) -{ - std::string true_param; - n.searchParam(param, true_param); - n.getParam(true_param, out); -} - -std::vector robotParam(ros::NodeHandle & n) -{ - std::string robot_str = ""; - getParam(n, "robot", robot_str); - if(robot_str.size() == 0) { return {}; } - if(robot_str[0] == '[') - { - std::vector ret = {""}; - for(size_t i = 1; i < robot_str.size(); ++i) - { - const auto & c = robot_str[i]; - if(c == ',') { ret.push_back(""); } - else if(c != ']' && c != ' ') { ret.back() += c; } - } - return ret; - } - return {robot_str}; -} -} // namespace - -int main(int argc, char * argv[]) -{ - ros::init(argc, argv, "log_visualizer", ros::init_options::NoSigintHandler); - auto nh = mc_rtc::ROSBridge::get_node_handle(); - if(!nh) { mc_rtc::log::error_and_throw("Failed to initialized node handle"); } - - ros::NodeHandle nh_private("~"); - - std::string log = ""; - getParam(nh_private, "log", log); - - mc_rbdyn::RobotModulePtr mod; - { - std::vector robot_params = robotParam(nh_private); - if(robot_params.size() == 1) { mod = mc_rbdyn::RobotLoader::get_robot_module(robot_params[0]); } - else if(robot_params.size() == 2) - { - mod = mc_rbdyn::RobotLoader::get_robot_module(robot_params[0], robot_params[1]); - } - else if(robot_params.size() == 3) - { - mod = mc_rbdyn::RobotLoader::get_robot_module(robot_params[0], robot_params[1], robot_params[2]); - } - else - { - mc_rtc::log::error_and_throw("log_visualization cannot handle the robot_params it was given"); - } - } - double dt = 0.005; - getParam(nh_private, "dt", dt); - - mc_rtc::log::info("Replaying log: {}", log); - LogPublisher appli(*nh, log, mod, dt); - appli.run(); - - return 0; -} diff --git a/mc_rtc_ticker/CMakeLists.txt b/mc_rtc_ticker/CMakeLists.txt index 7546a37..5ade407 100644 --- a/mc_rtc_ticker/CMakeLists.txt +++ b/mc_rtc_ticker/CMakeLists.txt @@ -2,7 +2,7 @@ # Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL # -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.10) set(CMAKE_CXX_STANDARD 17) set(PROJECT_NAME mc_rtc_ticker) project( @@ -13,21 +13,4 @@ project( find_package(catkin REQUIRED) catkin_package() -find_package(mc_rtc REQUIRED) - -include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR}) - -set(MC_RTC_TICKER_SRC src/mc_rtc_ticker.cpp) - -set(MC_RTC_TICKER_HDR) - -add_executable(mc_rtc_ticker ${MC_RTC_TICKER_SRC} ${MC_RTC_TICKER_HDR}) -target_link_libraries(mc_rtc_ticker mc_rtc::mc_control mc_rtc::mc_rtc_ros) - -install( - TARGETS mc_rtc_ticker - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/mc_rtc_ticker/etc/mc_rtc.conf b/mc_rtc_ticker/etc/mc_rtc.conf deleted file mode 100644 index 5c1908e..0000000 --- a/mc_rtc_ticker/etc/mc_rtc.conf +++ /dev/null @@ -1,29 +0,0 @@ -{ - // What robot is being controlled - "MainRobot" : "HRP2DRC", - - "Enabled" : ["CoM"], - - // Publish the state of the controlled robot (ROS) - "PublishControlState" : true, - - // Publish the state of the real robot (ROS) - "PublishRealState" : true, - - // Timestep of publication (ROS) - "PublishTimestep" : 0.01, - - // Controller timestep - "Timestep" : 0.005, - - // Set to true or false to enable/disable respectively - "Log" : true, - - // LogDirectory dictates where the log files will be stored, defaults to system temp directory - // "LogDirectory" : "/tmp", - - // The log file will have the name [LogTemplate]-[ControllerName]-[date].log - "LogTemplate" : "mc-control", - - "UseSandbox" : false -} diff --git a/mc_rtc_ticker/launch/control_display.launch b/mc_rtc_ticker/launch/control_display.launch index 935074d..2086d15 100644 --- a/mc_rtc_ticker/launch/control_display.launch +++ b/mc_rtc_ticker/launch/control_display.launch @@ -1,24 +1,3 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/mc_rtc_ticker/src/mc_rtc_ticker.cpp b/mc_rtc_ticker/src/mc_rtc_ticker.cpp deleted file mode 100644 index 12e4c41..0000000 --- a/mc_rtc_ticker/src/mc_rtc_ticker.cpp +++ /dev/null @@ -1,246 +0,0 @@ -/* - * Copyright 2016-2022 CNRS-UM LIRMM, CNRS-AIST JRL - */ - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -namespace -{ -template -T getParam(ros::NodeHandle & n, const std::string & param) -{ - T out; - std::string true_param; - n.searchParam(param, true_param); - n.getParam(true_param, out); - return out; -} - -template -T getParam(ros::NodeHandle & n, const std::string & param, const T & def) -{ - if(n.hasParam(param)) { return getParam(n, param); } - return def; -} -} // namespace - -int main() -{ - auto nh_p = mc_rtc::ROSBridge::get_node_handle(); - if(!nh_p) { return 1; } - auto nh = *nh_p; - - std::string conf = ""; - if(nh.hasParam("mc_rtc_ticker/conf")) - { - conf = getParam(nh, "mc_rtc_ticker/conf"); - mc_rtc::log::info("Configuring mc_rtc with {}", conf); - } - - if(mc_rtc::MC_RTC_VERSION != mc_rtc::version()) - { - mc_rtc::log::error("mc_rtc_ticker was compiled with {} but mc_rtc is at version {}, you might face subtle issues " - "or unexpected crashes, please recompile mc_rtc_ticker", - mc_rtc::MC_RTC_VERSION, mc_rtc::version()); - } - - mc_control::MCGlobalController controller(conf); - double dt = controller.timestep(); - - std::vector q; - if(nh.hasParam("mc_rtc_ticker/init_state")) { q = getParam>(nh, "mc_rtc_ticker/init_state"); } - else - { - auto & mbc = controller.robot().mbc(); - const auto & rjo = controller.ref_joint_order(); - for(const auto & jn : rjo) - { - if(controller.robot().hasJoint(jn)) - { - auto qIndex = controller.robot().jointIndexByName(jn); - const auto & qi = mbc.q[qIndex]; - if(qi.size()) - { - for(auto & qj : qi) { q.push_back(qj); } - } - else { q.push_back(0.0); } - } - else - { - // FIXME This assumes that a joint that is in ref_joint_order but missing from the robot is of size 1 (very - // likely to be true) - q.push_back(0); - } - } - } - controller.setEncoderValues(q); - if(nh.hasParam("mc_rtc_ticker/init_pos")) - { - mc_rtc::log::info("Using initial pos from ROS param"); - std::vector pos = getParam>(nh, "mc_rtc_ticker/init_pos"); - for(const auto & pi : pos) { std::cout << pi << " "; } - std::cout << std::endl; - std::array p; - std::copy_n(pos.begin(), 7, p.begin()); - controller.init(q, p); - } - else { controller.init(q); } - controller.running = true; - - bool stepByStep = getParam(nh, "mc_rtc_ticker/stepByStep", false); - size_t nextStep = 0; - auto gui = controller.controller().gui(); - auto toogleStepByStep = [&]() - { - if(stepByStep) { stepByStep = false; } - else - { - nextStep = 0; - stepByStep = true; - } - }; - if(gui) - { - gui->addElement({"mc_rtc_ticker"}, - mc_rtc::gui::Checkbox( - "Step by step", [&]() { return stepByStep; }, [&]() { toogleStepByStep(); })); - auto buttonText = [&](size_t n) - { - size_t n_ms = static_cast(std::ceil(static_cast(n) * 1000.0 * dt)); - return "+" + std::to_string(n_ms) + "ms"; - }; - gui->addElement({"mc_rtc_ticker"}, mc_rtc::gui::ElementsStacking::Horizontal, - mc_rtc::gui::Button(buttonText(1), [&]() { nextStep = 1; }), - mc_rtc::gui::Button(buttonText(5), [&]() { nextStep = 5; }), - mc_rtc::gui::Button(buttonText(10), [&]() { nextStep = 10; }), - mc_rtc::gui::Button(buttonText(50), [&]() { nextStep = 20; }), - mc_rtc::gui::Button(buttonText(100), [&]() { nextStep = 100; })); - } - - const bool bench = getParam(nh, "mc_rtc_ticker/bench", false); - std::chrono::time_point begin, end; - std::vector> solve_dt(0); - if(bench) - { - /* Reserve a million entry */ - solve_dt.reserve(1000000); - } - auto report_dt = [&solve_dt](bool partial) - { - std::cout << "Ran for " << solve_dt.size() << " iterations: "; - size_t start_i = partial ? solve_dt.size() - 1000 : 0; - auto min_t = solve_dt[start_i]; - auto max_t = solve_dt[start_i]; - auto second_max_t = solve_dt[start_i]; - std::chrono::duration average_t(0); - for(size_t i = start_i; i < solve_dt.size(); ++i) - { - const auto & t = solve_dt[i]; - if(t > max_t) - { - second_max_t = max_t; - max_t = t; - } - if(t < min_t) { min_t = t; } - average_t += t; - } - if(partial) { average_t = average_t / 1000; } - else { average_t = average_t / solve_dt.size(); } - std::cout << " avg: " << average_t.count() * 1000 << "ms, min: " << min_t.count() * 1000 - << "ms, max: " << max_t.count() * 1000 << "ms, second max: " << second_max_t.count() * 1000 << std::endl; - }; - - ros::Rate rt(1 / dt); - std::thread spin_th; - if(bench) - { - spin_th = std::thread( - [&rt]() - { - while(ros::ok()) - { - ros::spinOnce(); - rt.sleep(); - } - }); - } - - auto runController = [&]() - { - controller.setEncoderValues(q); - controller.run(); - }; - - auto updateGUI = [&]() - { - controller.running = false; - controller.run(); - controller.running = true; - }; - - while(ros::ok()) - { - if(bench) { begin = std::chrono::system_clock::now(); } - auto & mbc = controller.robot().mbc(); - const auto & rjo = controller.ref_joint_order(); - size_t index = 0; - for(size_t i = 0; i < rjo.size(); ++i) - { - const auto & jn = rjo[i]; - if(controller.robot().hasJoint(jn)) - { - auto qIndex = controller.robot().jointIndexByName(jn); - const auto & qi = mbc.q[qIndex]; - if(qi.size()) - { - for(const auto & qj : qi) - { - q[index] = qj; - index++; - } - } - else { index++; } - } - else { index++; } - } - if(stepByStep) - { - if(nextStep > 0) - { - nextStep--; - runController(); - } - else { updateGUI(); } - } - else { runController(); } - if(bench) - { - end = std::chrono::system_clock::now(); - solve_dt.push_back(end - begin); - if(solve_dt.size() % 1000 == 0) { report_dt(true); } - } - else - { - ros::spinOnce(); - rt.sleep(); - } - } - - if(bench) - { - report_dt(false); - spin_th.join(); - } - - return 0; -} diff --git a/mc_surfaces_visualization/CMakeLists.txt b/mc_surfaces_visualization/CMakeLists.txt deleted file mode 100644 index 596328a..0000000 --- a/mc_surfaces_visualization/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -# -# Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL -# - -cmake_minimum_required(VERSION 3.1) -set(CMAKE_CXX_STANDARD 17) -set(PROJECT_NAME mc_surfaces_visualization) -project( - ${PROJECT_NAME} - LANGUAGES CXX - VERSION 1.5.0) - -find_package(catkin REQUIRED COMPONENTS visualization_msgs geometry_msgs roscpp - std_msgs) - -catkin_package() - -find_package(mc_rtc REQUIRED) - -include_directories(${catkin_INCLUDE_DIRS}) - -set(src_files src/main.cpp) - -add_executable(surfaces_publisher ${src_files}) -target_link_libraries( - surfaces_publisher PUBLIC mc_rtc::mc_rbdyn mc_rtc::mc_rtc_ros - ${catkin_LIBRARIES}) - -install( - TARGETS surfaces_publisher - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/mc_surfaces_visualization/README.md b/mc_surfaces_visualization/README.md deleted file mode 100644 index ac4d6e3..0000000 --- a/mc_surfaces_visualization/README.md +++ /dev/null @@ -1,30 +0,0 @@ -# mc\_surfaces\_visualization - -This tool allows to visualize the surfaces of an mc\_rtc robot in the RViZ application. - -## Launching the visualization - -The visualization is launched as follows: - -```bash -$ roslaunch mc_surfaces_visualization display.launch robot:=JVRC1 -``` - -The `robot` argument should be the same as what you would pass to the `mc_rbdyn::RobotLoader::get_robot_module` function, this can also be a robot alias. For example: - -```bash -# Specify a vector of arguments -$ roslaunch mc_surfaces_visualization display.launch robot:="[env, `rospack find mc_env_description`, ground]" -# Or an alias -$ roslaunch mc_surfaces_visualization display.launch robot:=env/ground -``` - -## Surfaces visualization - -In the visualization: - -- planar surfaces are drawn in green with a blue arrow showing the normal direction of the surface -- cylindrical surfaces are green cylinders -- gripper surfaces are represented with blue arrows representing the normal direction of the gripper's points' frames - -You can show/hide selected surfaces by enabling/disabling their namespaces in the associated MarkerArray diff --git a/mc_surfaces_visualization/launch/display.launch b/mc_surfaces_visualization/launch/display.launch deleted file mode 100644 index cb89038..0000000 --- a/mc_surfaces_visualization/launch/display.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/mc_surfaces_visualization/launch/surfaces.rviz b/mc_surfaces_visualization/launch/surfaces.rviz deleted file mode 100644 index 487e497..0000000 --- a/mc_surfaces_visualization/launch/surfaces.rviz +++ /dev/null @@ -1,152 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Grid1 - - /MarkerArray1 - - /MarkerArray1/Status1 - - /RobotModel1 - Splitter Ratio: 0.5 - Tree Height: 775 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /surfaces - Name: MarkerArray - Namespaces: - Door: true - Handle: true - Queue Size: 1000 - Value: true - - Alpha: 0.5 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - door: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - handle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: robot_map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 1.75763 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.0814009 - Y: 0.778704 - Z: 0.95209 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.390398 - Target Frame: - Value: Orbit (rviz) - Yaw: 5.39358 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1855 - X: 1985 - Y: 24 diff --git a/mc_surfaces_visualization/launch/surfaces_publisher.launch b/mc_surfaces_visualization/launch/surfaces_publisher.launch deleted file mode 100644 index 88ff231..0000000 --- a/mc_surfaces_visualization/launch/surfaces_publisher.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/mc_surfaces_visualization/package.xml b/mc_surfaces_visualization/package.xml deleted file mode 100644 index 2348788..0000000 --- a/mc_surfaces_visualization/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - mc_surfaces_visualization - 1.5.0 - Visualize contact surfaces in RVIz - - Hervé Audren - Pierre Gergondet - - BSD - - catkin - - roscpp - std_msgs - geometry_msgs - visualization_msgs - - roscpp - std_msgs - geometry_msgs - visualization_msgs - diff --git a/mc_surfaces_visualization/src/main.cpp b/mc_surfaces_visualization/src/main.cpp deleted file mode 100644 index c365f77..0000000 --- a/mc_surfaces_visualization/src/main.cpp +++ /dev/null @@ -1,254 +0,0 @@ -/* - * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL - */ - -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include - -namespace -{ -template -void getParam(ros::NodeHandle & n, const std::string & param, T & out) -{ - std::string true_param; - n.searchParam(param, true_param); - n.getParam(true_param, out); -} - -std::vector robotParam(ros::NodeHandle & n) -{ - std::string robot_str = ""; - getParam(n, "robot", robot_str); - if(robot_str.size() == 0) { return {}; } - if(robot_str[0] == '[') - { - std::vector ret = {""}; - for(size_t i = 1; i < robot_str.size(); ++i) - { - const auto & c = robot_str[i]; - if(c == ',') { ret.push_back(""); } - else if(c != ']' && c != ' ') { ret.back() += c; } - } - return ret; - } - return {robot_str}; -} - -visualization_msgs::MarkerArray surfaceMarkers(const std::string & tf_prefix, const mc_rbdyn::Robot & robot) -{ - visualization_msgs::MarkerArray markers; - unsigned id = 0; - for(const auto & pair : robot.surfaces()) - { - - std::shared_ptr surf = pair.second; - const std::string & frame = surf->name(); - - visualization_msgs::Marker marker; - marker.header.frame_id = tf_prefix + "surfaces/" + surf->name(); - marker.header.stamp = ros::Time::now(); - marker.ns = frame; - marker.id = ++id; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = 0; - marker.pose.position.y = 0; - marker.pose.position.z = 0; - - marker.scale.x = 1.0; - marker.scale.y = 1.0; - marker.scale.z = 1.0; - marker.color.a = 1.0; // Don't forget to set the alpha! - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - marker.lifetime = ros::Duration(1); - - if(surf->type() == "cylindrical") - { - marker.type = visualization_msgs::Marker::CYLINDER; - mc_rbdyn::CylindricalSurface * cs = dynamic_cast(surf.get()); - marker.scale.x = 2 * cs->radius(); - marker.scale.y = 2 * cs->radius(); - marker.scale.z = cs->width(); - Eigen::Quaterniond new_ori = Eigen::Quaterniond(Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitY())); - marker.pose.orientation.x = new_ori.x(); - marker.pose.orientation.y = new_ori.y(); - marker.pose.orientation.z = new_ori.z(); - marker.pose.orientation.w = new_ori.w(); - - markers.markers.push_back(marker); - } - else if(surf->type() == "planar") - { - marker.type = visualization_msgs::Marker::LINE_STRIP; - const std::vector> & points = - dynamic_cast(surf.get())->planarPoints(); - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - geometry_msgs::Point p; - for(const auto & point : points) - { - p.x = point.first; - p.y = point.second; - p.z = 0.0; - marker.points.push_back(p); - } - marker.points.push_back(marker.points[0]); - - marker.scale.x = 0.01; - marker.scale.y = 0; - marker.scale.z = 0; - - markers.markers.push_back(marker); - - auto normal = marker; - normal.id = ++id; - normal.ns += "_normal"; - normal.type = visualization_msgs::Marker::ARROW; - normal.scale.x = 0.01; - normal.scale.y = 0.02; - normal.scale.z = 0.1; - normal.color.g = 0.0; - normal.color.b = 1.0; - normal.points.clear(); - p.x = 0; - p.y = 0; - p.z = 0; - normal.points.push_back(p); - p.x = 0; - p.y = 0; - p.z = 0.2; - normal.points.push_back(p); - markers.markers.push_back(normal); - } - else if(surf->type() == "gripper") - { - marker.type = visualization_msgs::Marker::ARROW; - marker.scale.x = 0.005; - marker.scale.y = 0.01; - marker.color.g = 0.0; - marker.color.b = 1.0; - marker.pose.orientation.w = 1; - marker.pose.orientation.x = 0; - marker.pose.orientation.y = 0; - marker.pose.orientation.z = 0; - const std::vector pFO = - dynamic_cast(surf.get())->pointsFromOrigin(); - for(const auto & p : pFO) - { - auto m = marker; - m.id = ++id; - auto p1 = p.translation(); - auto p2 = p1 + p.rotation().col(2) * 0.05; - geometry_msgs::Point gp; - gp.x = p1.x(); - gp.y = p1.y(); - gp.z = p1.z(); - m.points.push_back(gp); - gp.x = p2.x(); - gp.y = p2.y(); - gp.z = p2.z(); - m.points.push_back(gp); - markers.markers.push_back(m); - } - } - } - return markers; -} - -mc_rbdyn::RobotModulePtr rmFromParam(const std::vector & robot_params) -{ - if(robot_params.size() == 1) { return mc_rbdyn::RobotLoader::get_robot_module(robot_params[0]); } - else if(robot_params.size() == 2) - { - return mc_rbdyn::RobotLoader::get_robot_module(robot_params[0], robot_params[1]); - } - else if(robot_params.size() == 3) - { - return mc_rbdyn::RobotLoader::get_robot_module(robot_params[0], robot_params[1], robot_params[2]); - } - else if(robot_params.size() > 3) - { - ROS_ERROR_STREAM("Invalid robot_params size passed to mc_surfaces_visualization: " << robot_params.size()); - } - return nullptr; -} - -} // namespace - -int main(int argc, char ** argv) -{ - ros::init(argc, argv, "surfaces_visualization"); - - ros::NodeHandle n; - ros::NodeHandle n_private("~"); - - std::vector robot_module = {}; - getParam(n, "robot_module", robot_module); - std::vector robot_param = robotParam(n_private); - std::vector robot_params = robot_param.size() ? robot_param : robot_module; - bool robot_set = robot_param.size() != 0; - double publish_rate = 100.; - - std::string tf_prefix = ""; - getParam(n, "tf_prefix", tf_prefix); - if(tf_prefix.size() && tf_prefix[tf_prefix.size() - 1] != '/') { tf_prefix += '/'; } - - bool publish = false; - getParam(n_private, "publish", publish); - getParam(n_private, "rate", publish_rate); - double dt = 1 / publish_rate; - - ros::Publisher surface_pub = n.advertise("surfaces", 1000); - - std::shared_ptr robotModule = nullptr; - std::unique_ptr robot_pub; - std::shared_ptr robots; - visualization_msgs::MarkerArray markers; - auto init = [&]() - { - robotModule = rmFromParam(robot_params); - if(!robotModule) { return; } - robots = mc_rbdyn::loadRobot(*robotModule); - markers = surfaceMarkers(tf_prefix, robots->robot()); - if(publish) - { - robot_pub.reset(new mc_rtc::RobotPublisher(tf_prefix, publish_rate, dt)); - robot_pub->init(robots->robot()); - } - }; - - init(); - - ros::Rate rate(publish_rate); - while(ros::ok()) - { - if(robot_pub) { robot_pub->update(dt, robots->robot()); } - auto now = ros::Time::now(); - for(auto & m : markers.markers) { m.header.stamp = now; } - surface_pub.publish(markers); - ros::spinOnce(); - getParam(n, "robot_module", robot_module); - if(!robot_set && robot_module.size() && robot_module != robot_params) - { - robot_params = robot_module; - init(); - } - rate.sleep(); - } - - return 0; -} From 7fb6539bb6289e28f095eea614b6f3b59f2b3646 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 15 Feb 2024 11:32:03 +0900 Subject: [PATCH 02/10] [ci] Simplify workflow --- .github/workflows/build.yml | 81 ++++--------------------------------- 1 file changed, 8 insertions(+), 73 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index c03ad90..ebb1189 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -20,86 +20,21 @@ jobs: fail-fast: false matrix: os: [ubuntu-20.04] - compiler: [gcc] runs-on: ${{ matrix.os }} steps: - - uses: actions/checkout@v1 - with: - submodules: recursive - - name: Temporary APT mirrors cleanup - if: startsWith(runner.os, 'Linux') - 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: startsWith(runner.os, 'Linux') - run: | - set -e - set -x - pushd . - if [ "${{ matrix.os }}" = "ubuntu-16.04" ] - then - export ROS_DISTRO="kinetic" - 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 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 - cd ../ - catkin_make || exit 1 - popd - - name: Setup env - if: matrix.os == 'macos-latest' - run: | - export PATH=/usr/local/opt/qt/bin:$PATH - echo "PATH=${PATH}" >> $GITHUB_ENV - export CMAKE_PREFIX_PATH=/usr/local/opt/qt@5:${CMAKE_PREFIX_PATH} - echo "CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}" >> $GITHUB_ENV - name: Install dependencies uses: jrl-umi3218/github-actions/install-dependencies@master with: - compiler: gcc - build-type: RelWithDebInfo ubuntu: | - apt: libltdl-dev qt5-default libqwt-qt5-dev libmc-rtc-dev - macos: | - 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 - macos-options: '-DCMAKE_CXX_STANDARD=17' + apt-mirrors: + mc-rtc: + cloudsmith: mc-rtc/head + apt: libmc-rtc-dev + ros: | + apt: mc-rtc-plugin - name: Build with catkin - 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 + if: ${{ matrix.os == 'ubuntu-20.04' }} + uses: jrl-umi3218/github-actions/build-catkin-project@master - name: Slack Notification if: failure() uses: archive/github-actions-slack@master From 568865997d78c917cf79839f5ba9f544d25ba423 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 15 Feb 2024 11:55:37 +0900 Subject: [PATCH 03/10] Move CMakeLists.txt out of the root folder --- README.md | 2 +- CMakeLists.txt => debian/CMakeLists.txt | 3 --- debian/rules | 4 ++++ mc_rtc_ros/CMakeLists.txt | 2 +- mc_rtc_rviz_panel/CMakeLists.txt | 2 +- 5 files changed, 7 insertions(+), 6 deletions(-) rename CMakeLists.txt => debian/CMakeLists.txt (64%) diff --git a/README.md b/README.md index 8e118f1..e2f4829 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ This stack requires: Installing ------ -## Ubuntu LTS (16.04, 18.04, 20.04) +## Ubuntu LTS (18.04, 20.04. 22.04) You must first setup our package mirror: diff --git a/CMakeLists.txt b/debian/CMakeLists.txt similarity index 64% rename from CMakeLists.txt rename to debian/CMakeLists.txt index 91bae7c..2b1ba8e 100644 --- a/CMakeLists.txt +++ b/debian/CMakeLists.txt @@ -7,10 +7,7 @@ enable_testing() find_package(catkin) if(${catkin_FOUND}) - add_subdirectory(mc_log_visualization) - add_subdirectory(mc_convex_visualization) add_subdirectory(mc_rtc_ticker) - add_subdirectory(mc_surfaces_visualization) endif() add_subdirectory(mc_rtc_rviz_panel) diff --git a/debian/rules b/debian/rules index 8c38fc8..1485e38 100755 --- a/debian/rules +++ b/debian/rules @@ -8,9 +8,11 @@ ifeq (${ROS_DISTRO},) export CMAKE_OPTIONS= export EXTRA_STEPS= else + export ROS_VERSION:=$(shell . /opt/ros/${ROS_DISTRO}/setup.sh && echo $$ROS_VERSION) export ROS_PYTHON_VERSION:=$(shell . /opt/ros/${ROS_DISTRO}/setup.sh && echo $$ROS_PYTHON_VERSION) ifeq (${ROS_PYTHON_VERSION}, 3) export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python3/dist-packages:${PYTHONPATH} + export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python$(shell python3 -c "import sys; print(f'{sys.version_info.major}.{sys.version_info.minor}');")/site-packages:${PYTHONPATH} export PYTHON_EXECUTABLE=/usr/bin/python3 else export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python2.7/dist-packages:${PYTHONPATH} @@ -19,6 +21,7 @@ else export PKG_CONFIG_PATH=/opt/ros/${ROS_DISTRO}/lib/pkgconfig:$PKG_CONFIG_PATH export ROS_MASTER_URI=http://localhost:11311 export ROS_PACKAGE_PATH=/opt/ros/${ROS_DISTRO}/share + export LD_LIBRARY_PATH:=/opt/ros/${ROS_DISTRO}/lib:$LD_LIBRARY_PATH export CMAKE_PREFIX_PATH=/opt/ros/${ROS_DISTRO}:$CMAKE_PREFIX_PATH export CMAKE_OPTIONS=-DCMAKE_INSTALL_PREFIX=/opt/ros/${ROS_DISTRO} -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE} endif @@ -28,6 +31,7 @@ endif dh $@ override_dh_auto_configure: + cp $(CURDIR)/debian/CMakeLists.txt $(CURDIR)/ dh_auto_configure -- ${CMAKE_OPTIONS} override_dh_auto_install: diff --git a/mc_rtc_ros/CMakeLists.txt b/mc_rtc_ros/CMakeLists.txt index 64da323..bddeed2 100644 --- a/mc_rtc_ros/CMakeLists.txt +++ b/mc_rtc_ros/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10) project(mc_rtc_ros) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/mc_rtc_rviz_panel/CMakeLists.txt b/mc_rtc_rviz_panel/CMakeLists.txt index 79bb9d3..ddbf891 100644 --- a/mc_rtc_rviz_panel/CMakeLists.txt +++ b/mc_rtc_rviz_panel/CMakeLists.txt @@ -2,7 +2,7 @@ # Copyright 2016-2023 CNRS-UM LIRMM, CNRS-AIST JRL # -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.10) set(PROJECT_NAME mc_rtc_rviz_panel) set(PROJECT_DESCRIPTION "mc_rtc RVIz panel for prototyping controllers") From 209d1e6fcac4ded9cfc070e8b7e1901e3946ac92 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 15 Feb 2024 11:56:15 +0900 Subject: [PATCH 04/10] [ci] Add missing dependency --- .github/workflows/build.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ebb1189..72464c2 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -29,7 +29,7 @@ jobs: apt-mirrors: mc-rtc: cloudsmith: mc-rtc/head - apt: libmc-rtc-dev + apt: libmc-rtc-dev qt5-default libqwt-qt5-dev ros: | apt: mc-rtc-plugin - name: Build with catkin From c1d8c9dfbe36e081c39b12a3cd56d9f55ea5553e Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 15 Feb 2024 14:46:06 +0900 Subject: [PATCH 05/10] [debian] Fix packaging after removal of mc_rtc_ticker executable --- debian/COLCON_IGNORE | 0 debian/ros-ROS_DISTRO-mc-rtc-ticker.install | 1 - 2 files changed, 1 deletion(-) create mode 100644 debian/COLCON_IGNORE diff --git a/debian/COLCON_IGNORE b/debian/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/debian/ros-ROS_DISTRO-mc-rtc-ticker.install b/debian/ros-ROS_DISTRO-mc-rtc-ticker.install index 9c9f422..0c40a3e 100644 --- a/debian/ros-ROS_DISTRO-mc-rtc-ticker.install +++ b/debian/ros-ROS_DISTRO-mc-rtc-ticker.install @@ -1,3 +1,2 @@ -opt/ros/@ROS_DISTRO@/lib/mc_rtc_ticker/* opt/ros/@ROS_DISTRO@/lib/pkgconfig/mc_rtc_ticker.pc opt/ros/@ROS_DISTRO@/share/mc_rtc_ticker/* From 3fd6107db2d9ab41159e7e4c38558b5429dd99f3 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Thu, 15 Feb 2024 15:55:06 +0900 Subject: [PATCH 06/10] Prepare transition to ROS2 - Add relevant COLCON_IGNORE file - Remove direct references to ROS types - Add new launch/plugin files for ROS2 All that is left should be to change code to compile with ROS2 --- mc_rtc_ros/COLCON_IGNORE | 0 mc_rtc_ros/package.xml | 3 - mc_rtc_rviz_panel/CMakeLists.txt | 46 +- mc_rtc_rviz_panel/package.xml | 50 +- mc_rtc_rviz_panel/plugin/rviz_plugin.xml | 9 + .../src/ArrowInteractiveMarkerWidget.cpp | 37 +- .../src/ArrowInteractiveMarkerWidget.h | 6 +- mc_rtc_rviz_panel/src/CMakeLists.txt | 97 +--- .../src/DisplayTrajectoryWidget.cpp | 12 +- .../src/DisplayTrajectoryWidget.h | 8 +- .../src/ForceInteractiveMarkerWidget.cpp | 20 +- .../src/ForceInteractiveMarkerWidget.h | 4 +- mc_rtc_rviz_panel/src/FormElement.cpp | 31 +- mc_rtc_rviz_panel/src/FormElement.h | 8 +- .../src/InteractiveMarkerWidget.cpp | 6 +- .../src/InteractiveMarkerWidget.h | 6 +- mc_rtc_rviz_panel/src/Panel.cpp | 11 +- mc_rtc_rviz_panel/src/Panel.h | 7 +- .../src/Point3DInteractiveMarkerWidget.cpp | 13 +- .../src/Point3DInteractiveMarkerWidget.h | 2 +- mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp | 18 +- mc_rtc_rviz_panel/src/PolygonMarkerWidget.h | 6 +- .../src/PolyhedronMarkerWidget.cpp | 40 +- .../src/PolyhedronMarkerWidget.h | 12 +- .../src/TransformInteractiveMarkerWidget.cpp | 31 +- .../src/TransformInteractiveMarkerWidget.h | 8 +- mc_rtc_rviz_panel/src/VisualWidget.cpp | 18 +- mc_rtc_rviz_panel/src/VisualWidget.h | 8 +- .../src/XYThetaInteractiveMarkerWidget.cpp | 18 +- .../src/XYThetaInteractiveMarkerWidget.h | 8 +- mc_rtc_rviz_panel/src/main.cpp | 10 + mc_rtc_rviz_panel/src/utils.cpp | 121 +++-- mc_rtc_rviz_panel/src/utils.h | 98 ++-- mc_rtc_ticker/CMakeLists.txt | 28 +- mc_rtc_ticker/launch/display.2.launch | 3 + mc_rtc_ticker/launch/display.2.rviz | 492 ++++++++++++++++++ 36 files changed, 909 insertions(+), 386 deletions(-) create mode 100644 mc_rtc_ros/COLCON_IGNORE create mode 100644 mc_rtc_rviz_panel/plugin/rviz_plugin.xml create mode 100644 mc_rtc_ticker/launch/display.2.launch create mode 100644 mc_rtc_ticker/launch/display.2.rviz diff --git a/mc_rtc_ros/COLCON_IGNORE b/mc_rtc_ros/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mc_rtc_ros/package.xml b/mc_rtc_ros/package.xml index d773d89..19ddfeb 100644 --- a/mc_rtc_ros/package.xml +++ b/mc_rtc_ros/package.xml @@ -6,11 +6,8 @@ Pierre Gergondet BSD catkin - mc_log_visualization mc_rtc_msgs mc_rtc_ticker - mc_surfaces_visualization - mc_convex_visualization diff --git a/mc_rtc_rviz_panel/CMakeLists.txt b/mc_rtc_rviz_panel/CMakeLists.txt index ddbf891..47aaa78 100644 --- a/mc_rtc_rviz_panel/CMakeLists.txt +++ b/mc_rtc_rviz_panel/CMakeLists.txt @@ -2,7 +2,8 @@ # Copyright 2016-2023 CNRS-UM LIRMM, CNRS-AIST JRL # -cmake_minimum_required(VERSION 3.10) +# Require 3.14 to use simplified linking with OBJECT libraries +cmake_minimum_required(VERSION 3.14) set(PROJECT_NAME mc_rtc_rviz_panel) set(PROJECT_DESCRIPTION "mc_rtc RVIz panel for prototyping controllers") @@ -14,18 +15,51 @@ project( LANGUAGES CXX VERSION 1.5.0) +if(NOT DEFINED ENV{ROS_VERSION}) + message(FATAL_ERROR "Expected to find ROS_VERSION in the environment") +endif() +set(ROS_VERSION $ENV{ROS_VERSION}) + find_package(mc_rtc REQUIRED) +if(NOT TARGET mc_rtc::mc_rtc_ros) + message(FATAL_ERROR "mc_rtc ROS plugin is required to build this project") +endif() set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/CMakeModules) find_package(Qwt) -find_package(catkin REQUIRED COMPONENTS roscpp tf rviz roslib) +macro(add_ros2_dependency PKG TARGET) + find_package(${PKG} REQUIRED) + target_link_libraries(ROSDependencies INTERFACE ${PKG}::${TARGET}) +endmacro() + +if(ROS_VERSION EQUAL 1) + find_package(catkin REQUIRED COMPONENTS roscpp tf rviz roslib) + + catkin_package(CATKIN_DEPENDS roscpp tf rviz roslib) + + include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) + link_directories(${catkin_LIBRARY_DIRS}) + + install(FILES rviz_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +elseif(ROS_VERSION EQUAL 2) + cmake_minimum_required(VERSION 3.22) + find_package(ament_cmake REQUIRED) -catkin_package(CATKIN_DEPENDS roscpp tf rviz roslib) + add_library(ROSDependencies INTERFACE IMPORTED) + add_ros2_dependency(rclcpp rclcpp) + add_ros2_dependency(tf2_ros tf2_ros) + add_ros2_dependency(visualization_msgs + visualization_msgs__rosidl_typesupport_cpp) + add_ros2_dependency(interactive_markers interactive_markers) + add_ros2_dependency(pluginlib pluginlib) + add_ros2_dependency(rviz_common rviz_common) -include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) -link_directories(${catkin_LIBRARY_DIRS}) + pluginlib_export_plugin_description_file(rviz_common plugin/rviz_plugin.xml) -install(FILES rviz_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + ament_package() +else() + message(FATAL_ERROR "ROS_VERSION ${ROS_VERSION} is not supported yet") +endif() add_subdirectory(src) diff --git a/mc_rtc_rviz_panel/package.xml b/mc_rtc_rviz_panel/package.xml index 087d859..c2c085c 100644 --- a/mc_rtc_rviz_panel/package.xml +++ b/mc_rtc_rviz_panel/package.xml @@ -1,23 +1,31 @@ - - mc_rtc_rviz_panel - 1.5.0 - mc_rtc generic edition panel - Pierre Gergondet - Stéphane Caron - Kevin Chappellet - BSD - - catkin - roscpp - rviz - roslib - tf - roslib - rviz - roscpp - tf - - - + + + mc_rtc_rviz_panel + 1.5.0 + mc_rtc generic edition panel + Pierre Gergondet + Stéphane Caron + Kevin Chappellet + BSD + https://github.com/jrl-umi3218/mc_rtc_ros + + catkin + ament_cmake + + roscpp + rviz + roslib + tf + + rclcpp + rviz2 + visualization_msgs + tf2_ros + + + + catkin + ament_cmake + diff --git a/mc_rtc_rviz_panel/plugin/rviz_plugin.xml b/mc_rtc_rviz_panel/plugin/rviz_plugin.xml new file mode 100644 index 0000000..2a0f24a --- /dev/null +++ b/mc_rtc_rviz_panel/plugin/rviz_plugin.xml @@ -0,0 +1,9 @@ + + + + mc_rtc interaction panel for RViz + + + diff --git a/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.cpp b/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.cpp index 2664ac7..b9f7124 100644 --- a/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.cpp @@ -3,31 +3,28 @@ namespace mc_rtc_rviz { -ArrowInteractiveMarkerWidget::ArrowInteractiveMarkerWidget( - const ClientWidgetParam & params, - const WidgetId & requestId, - std::shared_ptr & server, - const Eigen::Vector3d & start, - const Eigen::Vector3d & end, - const mc_rtc::gui::ArrowConfig & config, - bool control_start, - bool control_end, - ClientWidget * label) +ArrowInteractiveMarkerWidget::ArrowInteractiveMarkerWidget(const ClientWidgetParam & params, + const WidgetId & requestId, + std::shared_ptr & server, + const Eigen::Vector3d & start, + const Eigen::Vector3d & end, + const mc_rtc::gui::ArrowConfig & config, + bool control_start, + bool control_end, + ClientWidget * label) : ClientWidget(params), request_id_(requestId), start_marker_(server, id2name(requestId), - vm::InteractiveMarker{}, - [this](const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) - { handleStartRequest(feedback); }), + InteractiveMarker{}, + [this](const InteractiveMarkerFeedbackConstPtr & feedback) { handleStartRequest(feedback); }), end_marker_(server, id2name(requestId), - vm::InteractiveMarker{}, - [this](const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) - { handleEndRequest(feedback); }), + InteractiveMarker{}, + [this](const InteractiveMarkerFeedbackConstPtr & feedback) { handleEndRequest(feedback); }), arrow_marker_(server, id2name(requestId), makeInteractiveMarker(id2name(requestId) + "_arrow", makeArrowMarker(start, end, config)), - [](const visualization_msgs::InteractiveMarkerFeedbackConstPtr &) {}) + [](const InteractiveMarkerFeedbackConstPtr &) {}) { if(config.start_point_scale > 0) @@ -84,16 +81,14 @@ void ArrowInteractiveMarkerWidget::update(const Eigen::Vector3d & start, } } -void ArrowInteractiveMarkerWidget::handleStartRequest( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) +void ArrowInteractiveMarkerWidget::handleStartRequest(const InteractiveMarkerFeedbackConstPtr & feedback) { arrow_points_.head<3>() = Eigen::Vector3d{feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z}; client().send_request(request_id_, arrow_points_); } -void ArrowInteractiveMarkerWidget::handleEndRequest( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) +void ArrowInteractiveMarkerWidget::handleEndRequest(const InteractiveMarkerFeedbackConstPtr & feedback) { arrow_points_.tail<3>() = Eigen::Vector3d{feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z}; diff --git a/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.h index e0b60e4..dee2152 100644 --- a/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.h @@ -13,7 +13,7 @@ struct ArrowInteractiveMarkerWidget : public ClientWidget public: ArrowInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, - std::shared_ptr & server, + std::shared_ptr & server, const Eigen::Vector3d & start, const Eigen::Vector3d & end, const mc_rtc::gui::ArrowConfig & config, @@ -24,8 +24,8 @@ struct ArrowInteractiveMarkerWidget : public ClientWidget void update(const Eigen::Vector3d & start, const Eigen::Vector3d & end, const mc_rtc::gui::ArrowConfig & c); protected: - virtual void handleStartRequest(const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback); - virtual void handleEndRequest(const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback); + virtual void handleStartRequest(const InteractiveMarkerFeedbackConstPtr & feedback); + virtual void handleEndRequest(const InteractiveMarkerFeedbackConstPtr & feedback); protected: WidgetId request_id_; diff --git a/mc_rtc_rviz_panel/src/CMakeLists.txt b/mc_rtc_rviz_panel/src/CMakeLists.txt index 0cd3968..690c079 100644 --- a/mc_rtc_rviz_panel/src/CMakeLists.txt +++ b/mc_rtc_rviz_panel/src/CMakeLists.txt @@ -121,83 +121,40 @@ add_library(_gui_objects OBJECT ${SOURCE_FILES}) target_compile_definitions(_gui_objects PRIVATE -DMC_RTC_DOCDIR="${MC_RTC_DOCDIR}") if(NOT ${FORCE_QT5} AND "${ROS_DISTRO}" STREQUAL "indigo") - target_include_directories( - _gui_objects - PRIVATE $) - target_compile_definitions( - _gui_objects - PRIVATE $) - target_include_directories( - _gui_objects - PRIVATE $) - target_compile_definitions( - _gui_objects - PRIVATE $) + target_link_libraries(_gui_objects PUBLIC Qt4::QtCore Qt4::QtGui) else() - target_include_directories( - _gui_objects - PRIVATE $) - target_compile_definitions( - _gui_objects - PRIVATE $) - target_include_directories( - _gui_objects - PRIVATE $) - target_compile_definitions( - _gui_objects - PRIVATE $) - target_include_directories( - _gui_objects - PRIVATE $) - target_compile_definitions( - _gui_objects - PRIVATE $) + target_link_libraries(_gui_objects PUBLIC Qt5::Core Qt5::Gui Qt5::Widgets) endif() target_compile_options(_gui_objects PRIVATE -fPIC) -target_include_directories( - _gui_objects PUBLIC $) -target_include_directories( - _gui_objects - PUBLIC - $) -target_compile_definitions( - _gui_objects - PUBLIC - $) -target_compile_features( - _gui_objects - PUBLIC $ -) -target_include_directories( - _gui_objects - PUBLIC $) -target_compile_definitions( - _gui_objects - PUBLIC $) -target_compile_features( - _gui_objects - PUBLIC $) +target_link_libraries(_gui_objects PUBLIC Qwt::Qwt mc_rtc::mc_control_client + mc_rtc::mc_rtc_ros) -add_library(${PROJECT_NAME} Panel.cpp ${HEADER_FILES} ${MOC_HEADERS} - $) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries( - ${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} mc_rtc::mc_rtc_ros - mc_rtc::mc_control_client Qwt::Qwt) - -target_link_libraries(${PROJECT_NAME} PUBLIC) -if(NOT ${FORCE_QT5} AND "${ROS_DISTRO}" STREQUAL "indigo") - target_link_libraries(${PROJECT_NAME} PUBLIC Qt4::QtCore Qt4::QtGui) -else() - target_link_libraries(${PROJECT_NAME} PUBLIC Qt5::Core Qt5::Gui Qt5::Widgets) +add_library(${PROJECT_NAME} Panel.cpp ${HEADER_FILES} ${MOC_HEADERS}) +if(ROS_VERSION EQUAL 1) + add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) +elseif(ROS_VERSION EQUAL 2) + target_link_libraries(${PROJECT_NAME} PUBLIC ROSDependencies) + target_compile_definitions( + ${PROJECT_NAME} PRIVATE RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY + MC_RTC_RVIZ_PANEL_IS_ROS2) endif() +target_link_libraries(${PROJECT_NAME} PUBLIC _gui_objects) add_executable(mc_rtc_gui main.cpp) target_link_libraries(mc_rtc_gui PUBLIC ${PROJECT_NAME}) target_compile_options(mc_rtc_gui PRIVATE -fPIC) -install( - TARGETS ${PROJECT_NAME} mc_rtc_gui - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +if(ROS_VERSION EQUAL 1) + install( + TARGETS ${PROJECT_NAME} mc_rtc_gui + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +elseif(ROS_VERSION EQUAL 2) + install( + TARGETS ${PROJECT_NAME} mc_rtc_gui + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +endif() diff --git a/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.cpp b/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.cpp index 14b1abc..d6311b2 100644 --- a/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.cpp +++ b/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.cpp @@ -9,8 +9,7 @@ namespace mc_rtc_rviz { -DisplayTrajectoryWidget::DisplayTrajectoryWidget(const ClientWidgetParam & params, - visualization_msgs::MarkerArray & markers) +DisplayTrajectoryWidget::DisplayTrajectoryWidget(const ClientWidgetParam & params, MarkerArray & markers) : ClientWidget(params), markers_(markers), visible_(visible()), was_visible_(visible_) { auto layout = new QHBoxLayout(this); @@ -26,7 +25,7 @@ DisplayTrajectoryWidget::DisplayTrajectoryWidget(const ClientWidgetParam & param DisplayTrajectoryWidget::~DisplayTrajectoryWidget() { configure({}); - path_.action = visualization_msgs::Marker::DELETE; + path_.action = Marker::DELETE; publish(); } @@ -113,8 +112,7 @@ void DisplayTrajectoryWidget::update(const sva::PTransformd & point, const mc_rt void DisplayTrajectoryWidget::configure(const mc_rtc::gui::LineConfig & config) { - path_.type = config.style == mc_rtc::gui::LineStyle::Dotted ? visualization_msgs::Marker::POINTS - : visualization_msgs::Marker::LINE_STRIP; + path_.type = config.style == mc_rtc::gui::LineStyle::Dotted ? Marker::POINTS : Marker::LINE_STRIP; path_.header.frame_id = "robot_map"; path_.header.stamp = ros::Time::now(); path_.pose.orientation.w = 1.0; @@ -127,7 +125,7 @@ void DisplayTrajectoryWidget::configure(const mc_rtc::gui::LineConfig & config) path_.color.g = static_cast(config.color.g); path_.color.b = static_cast(config.color.b); path_.color.a = static_cast(config.color.a); - path_.action = visualization_msgs::Marker::ADD; + path_.action = Marker::ADD; path_.ns = id2name(id()); } @@ -136,7 +134,7 @@ void DisplayTrajectoryWidget::publish() if(visible_ || was_visible_) { markers_.markers.push_back(path_); - if(!visible_ || path_.points.size() == 0) { markers_.markers.back().action = visualization_msgs::Marker::DELETE; } + if(!visible_ || path_.points.size() == 0) { markers_.markers.back().action = Marker::DELETE; } } was_visible_ = visible_; } diff --git a/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.h b/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.h index c52235d..ee2ba8d 100644 --- a/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.h +++ b/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.h @@ -9,8 +9,6 @@ #include -#include - namespace mc_rtc_rviz { @@ -18,7 +16,7 @@ struct DisplayTrajectoryWidget : public ClientWidget { Q_OBJECT public: - DisplayTrajectoryWidget(const ClientWidgetParam & params, visualization_msgs::MarkerArray & markers); + DisplayTrajectoryWidget(const ClientWidgetParam & params, MarkerArray & markers); ~DisplayTrajectoryWidget() override; @@ -28,8 +26,8 @@ struct DisplayTrajectoryWidget : public ClientWidget void update(const sva::PTransformd & point, const mc_rtc::gui::LineConfig & config); private: - visualization_msgs::MarkerArray & markers_; - visualization_msgs::Marker path_; + MarkerArray & markers_; + Marker path_; void configure(const mc_rtc::gui::LineConfig & config); void publish(); bool visible_; diff --git a/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.cpp b/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.cpp index 08f61b4..ed0a478 100644 --- a/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.cpp @@ -3,15 +3,14 @@ namespace mc_rtc_rviz { -ForceInteractiveMarkerWidget::ForceInteractiveMarkerWidget( - const ClientWidgetParam & params, - const WidgetId & requestId, - std::shared_ptr & server, - const sva::PTransformd & surface, - const sva::ForceVecd & force, - const mc_rtc::gui::ForceConfig & config, - bool ro, - ClientWidget * label) +ForceInteractiveMarkerWidget::ForceInteractiveMarkerWidget(const ClientWidgetParam & params, + const WidgetId & requestId, + std::shared_ptr & server, + const sva::PTransformd & surface, + const sva::ForceVecd & force, + const mc_rtc::gui::ForceConfig & config, + bool ro, + ClientWidget * label) : ArrowInteractiveMarkerWidget(params, requestId, server, @@ -37,8 +36,7 @@ void ForceInteractiveMarkerWidget::update(const sva::PTransformd & surface, ArrowInteractiveMarkerWidget::update(start, end, c); } -void ForceInteractiveMarkerWidget::handleEndRequest( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) +void ForceInteractiveMarkerWidget::handleEndRequest(const InteractiveMarkerFeedbackConstPtr & feedback) { Eigen::Vector3d end = Eigen::Vector3d{feedback->pose.position.x, feedback->pose.position.y, feedback->pose.position.z}; diff --git a/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.h index c879f09..256cdc3 100644 --- a/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.h @@ -12,7 +12,7 @@ struct ForceInteractiveMarkerWidget : public ArrowInteractiveMarkerWidget public: ForceInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, - std::shared_ptr & server, + std::shared_ptr & server, const sva::PTransformd & surface, const sva::ForceVecd & force, const mc_rtc::gui::ForceConfig & config, @@ -22,7 +22,7 @@ struct ForceInteractiveMarkerWidget : public ArrowInteractiveMarkerWidget void update(const sva::PTransformd & surface, const sva::ForceVecd & force, const mc_rtc::gui::ForceConfig & c); protected: - void handleEndRequest(const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) override; + void handleEndRequest(const InteractiveMarkerFeedbackConstPtr & feedback) override; protected: mc_rtc::gui::ForceConfig config_; diff --git a/mc_rtc_rviz_panel/src/FormElement.cpp b/mc_rtc_rviz_panel/src/FormElement.cpp index 6f0171d..4065657 100644 --- a/mc_rtc_rviz_panel/src/FormElement.cpp +++ b/mc_rtc_rviz_panel/src/FormElement.cpp @@ -697,14 +697,13 @@ static std::string next_marker_name() } template -InteractiveMarkerInput::InteractiveMarkerInput( - QWidget * parent, - const std::string & name, - bool required, - const DataT & default_, - bool default_from_user, - bool interactive, - std::shared_ptr int_server) +InteractiveMarkerInput::InteractiveMarkerInput(QWidget * parent, + const std::string & name, + bool required, + const DataT & default_, + bool default_from_user, + bool interactive, + std::shared_ptr int_server) : FormElement(parent, name, required), marker_(int_server, next_marker_name(), @@ -713,7 +712,7 @@ InteractiveMarkerInput::InteractiveMarkerInput( (!rotation_only) && interactive, std::is_same_v && interactive), - [this](const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) { handleRequest(feedback); }), + [this](const InteractiveMarkerFeedbackConstPtr & feedback) { handleRequest(feedback); }), data_(default_), interactive_(interactive), server_(int_server) { form_interactive_input_id++; @@ -785,12 +784,11 @@ InteractiveMarkerInput::InteractiveMarkerInput( } template -void InteractiveMarkerInput::changed( - bool required, - const DataT & default_, - bool default_from_user, - bool interactive, - std::shared_ptr int_server) +void InteractiveMarkerInput::changed(bool required, + const DataT & default_, + bool default_from_user, + bool interactive, + std::shared_ptr int_server) { if(std::any_of(edits_.begin(), edits_.end(), [](const auto * edit) { return edit->hasFocus(); })) { return; } changed_(required); @@ -826,8 +824,7 @@ void InteractiveMarkerInput::fill(const mc_rtc::Configurat } template -void InteractiveMarkerInput::handleRequest( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) +void InteractiveMarkerInput::handleRequest(const InteractiveMarkerFeedbackConstPtr & feedback) { locked_ = true; ready_ = true; diff --git a/mc_rtc_rviz_panel/src/FormElement.h b/mc_rtc_rviz_panel/src/FormElement.h index 3bec598..430f8ba 100644 --- a/mc_rtc_rviz_panel/src/FormElement.h +++ b/mc_rtc_rviz_panel/src/FormElement.h @@ -570,13 +570,13 @@ struct InteractiveMarkerInput : public FormElement const DataT & default_, bool default_from_user, bool interactive, - std::shared_ptr int_server); + std::shared_ptr int_server); void changed(bool required, const DataT & default_, bool default_from_user, bool interactive, - std::shared_ptr int_server); + std::shared_ptr int_server); mc_rtc::Configuration serialize() const override; @@ -590,9 +590,9 @@ struct InteractiveMarkerInput : public FormElement DataT data_; bool user_default_; bool interactive_; - std::shared_ptr server_; + std::shared_ptr server_; - void handleRequest(const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback); + void handleRequest(const InteractiveMarkerFeedbackConstPtr & feedback); static constexpr size_t n_edits = std::is_same_v ? 3 : (rotation_only ? 4 : 7); std::array edits_; diff --git a/mc_rtc_rviz_panel/src/InteractiveMarkerWidget.cpp b/mc_rtc_rviz_panel/src/InteractiveMarkerWidget.cpp index 1e531ce..c165a05 100644 --- a/mc_rtc_rviz_panel/src/InteractiveMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/InteractiveMarkerWidget.cpp @@ -9,14 +9,14 @@ namespace mc_rtc_rviz InteractiveMarkerWidget::InteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, - std::shared_ptr & server, - const vm::InteractiveMarker & marker, + std::shared_ptr & server, + const InteractiveMarker & marker, ClientWidget * label) : ClientWidget(params), request_id_(requestId), marker_(server, id2name(requestId), marker, - [this](const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) { handleRequest(feedback); }) + [this](const InteractiveMarkerFeedbackConstPtr & feedback) { handleRequest(feedback); }) { layout_ = new QVBoxLayout(this); button_ = label->showHideButton(); diff --git a/mc_rtc_rviz_panel/src/InteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/InteractiveMarkerWidget.h index 77c6ea5..e0d0e25 100644 --- a/mc_rtc_rviz_panel/src/InteractiveMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/InteractiveMarkerWidget.h @@ -17,12 +17,12 @@ struct InteractiveMarkerWidget : public ClientWidget public: InteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, - std::shared_ptr & server, - const vm::InteractiveMarker & marker, + std::shared_ptr & server, + const InteractiveMarker & marker, ClientWidget * label); protected: - virtual void handleRequest(const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) = 0; + virtual void handleRequest(const InteractiveMarkerFeedbackConstPtr & feedback) = 0; protected: WidgetId request_id_; diff --git a/mc_rtc_rviz_panel/src/Panel.cpp b/mc_rtc_rviz_panel/src/Panel.cpp index d7b7898..a935680 100644 --- a/mc_rtc_rviz_panel/src/Panel.cpp +++ b/mc_rtc_rviz_panel/src/Panel.cpp @@ -112,16 +112,13 @@ double getTimeout() struct PanelImpl { - PanelImpl() - : nh_(), - int_server_(std::make_shared("mc_rtc_rviz_interactive_markers")) + PanelImpl() : nh_(), int_server_(std::make_shared("mc_rtc_rviz_interactive_markers")) { - marker_array_pub_ = - mc_rtc::ROSBridge::get_node_handle()->advertise("/mc_rtc_rviz", 0); + marker_array_pub_ = mc_rtc::ROSBridge::get_node_handle()->advertise("/mc_rtc_rviz", 0); } ros::NodeHandle nh_; - std::shared_ptr int_server_; - visualization_msgs::MarkerArray marker_array_; + std::shared_ptr int_server_; + MarkerArray marker_array_; ros::Publisher marker_array_pub_; }; diff --git a/mc_rtc_rviz_panel/src/Panel.h b/mc_rtc_rviz_panel/src/Panel.h index 8fde166..82963c1 100644 --- a/mc_rtc_rviz_panel/src/Panel.h +++ b/mc_rtc_rviz_panel/src/Panel.h @@ -14,12 +14,7 @@ #endif -#include -#include - -#include -#include -#include +#include namespace mc_rtc_rviz { diff --git a/mc_rtc_rviz_panel/src/Point3DInteractiveMarkerWidget.cpp b/mc_rtc_rviz_panel/src/Point3DInteractiveMarkerWidget.cpp index 8c21a23..5f91b90 100644 --- a/mc_rtc_rviz_panel/src/Point3DInteractiveMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/Point3DInteractiveMarkerWidget.cpp @@ -3,13 +3,12 @@ namespace mc_rtc_rviz { -Point3DInteractiveMarkerWidget::Point3DInteractiveMarkerWidget( - const ClientWidgetParam & params, - const WidgetId & requestId, - std::shared_ptr & server, - const mc_rtc::gui::PointConfig & config, - bool control_position, - ClientWidget * label) +Point3DInteractiveMarkerWidget::Point3DInteractiveMarkerWidget(const ClientWidgetParam & params, + const WidgetId & requestId, + std::shared_ptr & server, + const mc_rtc::gui::PointConfig & config, + bool control_position, + ClientWidget * label) : TransformInteractiveMarkerWidget(params, requestId, server, diff --git a/mc_rtc_rviz_panel/src/Point3DInteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/Point3DInteractiveMarkerWidget.h index ccbfc31..af4f3fb 100644 --- a/mc_rtc_rviz_panel/src/Point3DInteractiveMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/Point3DInteractiveMarkerWidget.h @@ -8,7 +8,7 @@ struct Point3DInteractiveMarkerWidget : public TransformInteractiveMarkerWidget { Point3DInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, - std::shared_ptr & server, + std::shared_ptr & server, const mc_rtc::gui::PointConfig & config, bool control_position, ClientWidget * label); diff --git a/mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp b/mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp index c91ec91..cf7ccda 100644 --- a/mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp @@ -9,7 +9,7 @@ namespace mc_rtc_rviz { -PolygonMarkerWidget::PolygonMarkerWidget(const ClientWidgetParam & params, visualization_msgs::MarkerArray & markers) +PolygonMarkerWidget::PolygonMarkerWidget(const ClientWidgetParam & params, MarkerArray & markers) : ClientWidget(params), markers_(markers), visible_(visible()), was_visible_(visible_) { auto layout = new QHBoxLayout(this); @@ -24,8 +24,8 @@ PolygonMarkerWidget::PolygonMarkerWidget(const ClientWidgetParam & params, visua PolygonMarkerWidget::~PolygonMarkerWidget() { - visualization_msgs::Marker m; - m.action = visualization_msgs::Marker::DELETE; + Marker m; + m.action = Marker::DELETE; m.ns = id2name(id()); for(size_t i = 0; i < currPolygonNum_; ++i) { @@ -53,9 +53,9 @@ void PolygonMarkerWidget::update(const std::string & ns, const std::vector & points, const mc_rtc::gui::LineConfig & c) { - visualization_msgs::Marker m; - m.type = visualization_msgs::Marker::LINE_STRIP; - m.action = visualization_msgs::Marker::ADD; + Marker m; + m.type = Marker::LINE_STRIP; + m.action = Marker::ADD; m.pose.position.x = 0; m.pose.position.y = 0; m.pose.position.z = 0; @@ -89,7 +89,7 @@ void PolygonMarkerWidget::update(const std::string & ns, if(visible_ || was_visible_) { markers_.markers.push_back(m); - if(!visible_) { markers_.markers.back().action = visualization_msgs::Marker::DELETE; } + if(!visible_) { markers_.markers.back().action = Marker::DELETE; } } was_visible_ = visible_; } @@ -98,8 +98,8 @@ void PolygonMarkerWidget::clear() { for(size_t i = currPolygonNum_; i < prevPolygonNum_; ++i) { - visualization_msgs::Marker m; - m.action = visualization_msgs::Marker::DELETE; + Marker m; + m.action = Marker::DELETE; m.ns = id2name(id()); m.id = static_cast(i); markers_.markers.push_back(m); diff --git a/mc_rtc_rviz_panel/src/PolygonMarkerWidget.h b/mc_rtc_rviz_panel/src/PolygonMarkerWidget.h index e0c9097..2b42bb9 100644 --- a/mc_rtc_rviz_panel/src/PolygonMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/PolygonMarkerWidget.h @@ -10,8 +10,6 @@ #include #include -#include - namespace mc_rtc_rviz { @@ -19,7 +17,7 @@ struct PolygonMarkerWidget : public ClientWidget { Q_OBJECT public: - PolygonMarkerWidget(const ClientWidgetParam & params, visualization_msgs::MarkerArray & markers); + PolygonMarkerWidget(const ClientWidgetParam & params, MarkerArray & markers); void update(const std::vector> & polygons, const mc_rtc::gui::LineConfig & c); @@ -33,7 +31,7 @@ struct PolygonMarkerWidget : public ClientWidget void clear(); private: - visualization_msgs::MarkerArray & markers_; + MarkerArray & markers_; size_t prevPolygonNum_ = 0; size_t currPolygonNum_ = 0; bool visible_; diff --git a/mc_rtc_rviz_panel/src/PolyhedronMarkerWidget.cpp b/mc_rtc_rviz_panel/src/PolyhedronMarkerWidget.cpp index 59b7e05..a0bb2dc 100644 --- a/mc_rtc_rviz_panel/src/PolyhedronMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/PolyhedronMarkerWidget.cpp @@ -10,7 +10,7 @@ namespace mc_rtc_rviz { PolyhedronMarkerWidget::PolyhedronMarkerWidget(const ClientWidgetParam & params, - visualization_msgs::MarkerArray & markers, + MarkerArray & markers, const mc_rtc::gui::PolyhedronConfig & config) : ClientWidget(params), markers_(markers), config_(config), visible_(visible()), visible_triangles_(config.show_triangle), visible_edges_(config.show_edges), visible_vertices_(config.show_vertices) @@ -38,8 +38,8 @@ PolyhedronMarkerWidget::PolyhedronMarkerWidget(const ClientWidgetParam & params, connect(show_edges_, SIGNAL(stateChanged(int)), this, SLOT(show_edges_changed(int))); connect(show_vertices_, SIGNAL(stateChanged(int)), this, SLOT(show_vertices_changed(int))); - triangles_.type = visualization_msgs::Marker::TRIANGLE_LIST; - triangles_.action = visualization_msgs::Marker::ADD; + triangles_.type = Marker::TRIANGLE_LIST; + triangles_.action = Marker::ADD; triangles_.header.frame_id = "robot_map"; triangles_.ns = id2name(id()) + "_triangles"; triangles_.id = 0; @@ -60,7 +60,7 @@ PolyhedronMarkerWidget::PolyhedronMarkerWidget(const ClientWidgetParam & params, edges_ = triangles_; edges_.ns = id2name(id()) + "_edges"; - edges_.type = visualization_msgs::Marker::LINE_LIST; + edges_.type = Marker::LINE_LIST; edges_.scale.x = config_.edge_config.width; edges_.scale.y = config_.edge_config.width; edges_.scale.z = config_.edge_config.width; @@ -71,7 +71,7 @@ PolyhedronMarkerWidget::PolyhedronMarkerWidget(const ClientWidgetParam & params, vertices_ = triangles_; vertices_.ns = id2name(id()) + "_vertices"; - vertices_.type = visualization_msgs::Marker::SPHERE_LIST; + vertices_.type = Marker::SPHERE_LIST; vertices_.id = 0; vertices_.scale.x = config_.vertices_config.scale; vertices_.scale.y = config_.vertices_config.scale; @@ -92,8 +92,8 @@ void PolyhedronMarkerWidget::update_triangles(const std::vector #include -#include - namespace mc_rtc_rviz { @@ -20,7 +18,7 @@ struct PolyhedronMarkerWidget : public ClientWidget Q_OBJECT public: PolyhedronMarkerWidget(const ClientWidgetParam & params, - visualization_msgs::MarkerArray & markers, + MarkerArray & markers, const mc_rtc::gui::PolyhedronConfig & config); void update(const std::vector> & triangles, @@ -43,14 +41,14 @@ private slots: void show_vertices_changed(int); private: - visualization_msgs::MarkerArray & markers_; + MarkerArray & markers_; mc_rtc::gui::PolyhedronConfig config_; QCheckBox * show_triangles_ = nullptr; QCheckBox * show_edges_ = nullptr; QCheckBox * show_vertices_ = nullptr; - visualization_msgs::Marker triangles_; // Triangles displayed as a TRIANGLE_LIST - visualization_msgs::Marker edges_; // Edges displayed as a LINE_STRIP - visualization_msgs::Marker vertices_; // Vertices displayed as a SPHERE_LIST + Marker triangles_; // Triangles displayed as a TRIANGLE_LIST + Marker edges_; // Edges displayed as a LINE_STRIP + Marker vertices_; // Vertices displayed as a SPHERE_LIST size_t prevPolyhedronNum_ = 0; size_t currPolyhedronNum_ = 0; bool visible_ = true; diff --git a/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.cpp b/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.cpp index c2318cf..3f8450b 100644 --- a/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.cpp @@ -3,13 +3,12 @@ namespace mc_rtc_rviz { -TransformInteractiveMarkerWidget::TransformInteractiveMarkerWidget( - const ClientWidgetParam & params, - const WidgetId & requestId, - std::shared_ptr & server, - bool control_orientation, - bool control_position, - ClientWidget * label) +TransformInteractiveMarkerWidget::TransformInteractiveMarkerWidget(const ClientWidgetParam & params, + const WidgetId & requestId, + std::shared_ptr & server, + bool control_orientation, + bool control_position, + ClientWidget * label) : InteractiveMarkerWidget( params, requestId, @@ -20,21 +19,19 @@ TransformInteractiveMarkerWidget::TransformInteractiveMarkerWidget( { } -TransformInteractiveMarkerWidget::TransformInteractiveMarkerWidget( - const ClientWidgetParam & params, - const WidgetId & requestId, - std::shared_ptr & server, - const vm::InteractiveMarker & marker, - bool control_orientation, - bool control_position, - ClientWidget * label) +TransformInteractiveMarkerWidget::TransformInteractiveMarkerWidget(const ClientWidgetParam & params, + const WidgetId & requestId, + std::shared_ptr & server, + const InteractiveMarker & marker, + bool control_orientation, + bool control_position, + ClientWidget * label) : InteractiveMarkerWidget(params, requestId, server, marker, label), control_orientation_(control_orientation), control_position_(control_position) { } -void TransformInteractiveMarkerWidget::handleRequest( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) +void TransformInteractiveMarkerWidget::handleRequest(const InteractiveMarkerFeedbackConstPtr & feedback) { if(!control_position_ && !control_orientation_) { return; } if(control_position_ && !control_orientation_) diff --git a/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.h index 535ff2a..fccb8d6 100644 --- a/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/TransformInteractiveMarkerWidget.h @@ -11,20 +11,20 @@ struct TransformInteractiveMarkerWidget : public InteractiveMarkerWidget public: TransformInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, - std::shared_ptr & server, + std::shared_ptr & server, bool control_orientation, bool control_position, ClientWidget * label); TransformInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, - std::shared_ptr & server, - const vm::InteractiveMarker & marker, + std::shared_ptr & server, + const InteractiveMarker & marker, bool control_orientation, bool control_position, ClientWidget * label); - void handleRequest(const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) override; + void handleRequest(const InteractiveMarkerFeedbackConstPtr & feedback) override; void update(const Eigen::Vector3d & t) { diff --git a/mc_rtc_rviz_panel/src/VisualWidget.cpp b/mc_rtc_rviz_panel/src/VisualWidget.cpp index 5269208..2542859 100644 --- a/mc_rtc_rviz_panel/src/VisualWidget.cpp +++ b/mc_rtc_rviz_panel/src/VisualWidget.cpp @@ -3,7 +3,7 @@ namespace mc_rtc_rviz { -VisualWidget::VisualWidget(const ClientWidgetParam & params, visualization_msgs::MarkerArray & markers) +VisualWidget::VisualWidget(const ClientWidgetParam & params, MarkerArray & markers) : ClientWidget(params), markers_(markers), visible_(visible()), was_visible_(visible_) { auto layout = new QHBoxLayout(this); @@ -21,7 +21,7 @@ VisualWidget::VisualWidget(const ClientWidgetParam & params, visualization_msgs: VisualWidget::~VisualWidget() { - marker_.action = visualization_msgs::Marker::DELETE; + marker_.action = Marker::DELETE; markers_.markers.push_back(marker_); } @@ -31,7 +31,7 @@ void VisualWidget::update(const rbd::parsers::Visual & visual, const sva::PTrans // Marker general settings marker_.header.seq++; marker_.header.stamp = ros::Time::now(); - marker_.action = visualization_msgs::Marker::ADD; + marker_.action = Marker::ADD; } { // Convert the position @@ -75,7 +75,7 @@ void VisualWidget::update(const rbd::parsers::Visual & visual, const sva::PTrans if(type == Type::BOX) { auto & b = boost::get(geom_data); - marker_.type = visualization_msgs::Marker::CUBE; + marker_.type = Marker::CUBE; marker_.scale.x = b.size.x(); marker_.scale.y = b.size.y(); marker_.scale.z = b.size.z(); @@ -83,7 +83,7 @@ void VisualWidget::update(const rbd::parsers::Visual & visual, const sva::PTrans else if(type == Type::CYLINDER) { auto & c = boost::get(geom_data); - marker_.type = visualization_msgs::Marker::CYLINDER; + marker_.type = Marker::CYLINDER; marker_.scale.x = 2 * c.radius; marker_.scale.y = 2 * c.radius; marker_.scale.z = c.length; @@ -91,7 +91,7 @@ void VisualWidget::update(const rbd::parsers::Visual & visual, const sva::PTrans else if(type == Type::SPHERE) { auto & s = boost::get(geom_data); - marker_.type = visualization_msgs::Marker::SPHERE; + marker_.type = Marker::SPHERE; marker_.scale.x = 2 * s.radius; marker_.scale.y = 2 * s.radius; marker_.scale.z = 2 * s.radius; @@ -99,7 +99,7 @@ void VisualWidget::update(const rbd::parsers::Visual & visual, const sva::PTrans else if(type == Type::MESH) { auto & m = boost::get(geom_data); - marker_.type = visualization_msgs::Marker::MESH_RESOURCE; + marker_.type = Marker::MESH_RESOURCE; marker_.scale.x = m.scaleV.x(); marker_.scale.y = m.scaleV.y(); marker_.scale.z = m.scaleV.z(); @@ -121,7 +121,7 @@ void VisualWidget::update(const rbd::parsers::Visual & visual, const sva::PTrans } else { - marker_.type = visualization_msgs::Marker::SPHERE; + marker_.type = Marker::SPHERE; marker_.scale.x = se.size.x(); marker_.scale.y = se.size.y(); marker_.scale.z = se.size.z(); @@ -130,7 +130,7 @@ void VisualWidget::update(const rbd::parsers::Visual & visual, const sva::PTrans } if(visible_ || was_visible_) { - if(!visible_) { marker_.action = visualization_msgs::Marker::DELETE; } + if(!visible_) { marker_.action = Marker::DELETE; } markers_.markers.push_back(marker_); } was_visible_ = visible_; diff --git a/mc_rtc_rviz_panel/src/VisualWidget.h b/mc_rtc_rviz_panel/src/VisualWidget.h index f822840..17fbd67 100644 --- a/mc_rtc_rviz_panel/src/VisualWidget.h +++ b/mc_rtc_rviz_panel/src/VisualWidget.h @@ -7,8 +7,6 @@ #include "ClientWidget.h" #include "utils.h" -#include - namespace mc_rtc_rviz { @@ -16,15 +14,15 @@ struct VisualWidget : public ClientWidget { Q_OBJECT public: - VisualWidget(const ClientWidgetParam & params, visualization_msgs::MarkerArray & markers); + VisualWidget(const ClientWidgetParam & params, MarkerArray & markers); ~VisualWidget() override; void update(const rbd::parsers::Visual & visual, const sva::PTransformd & pose); private: - visualization_msgs::MarkerArray & markers_; - visualization_msgs::Marker marker_; + MarkerArray & markers_; + Marker marker_; bool visible_; bool was_visible_; QPushButton * button_; diff --git a/mc_rtc_rviz_panel/src/XYThetaInteractiveMarkerWidget.cpp b/mc_rtc_rviz_panel/src/XYThetaInteractiveMarkerWidget.cpp index 27a822b..6c142ce 100644 --- a/mc_rtc_rviz_panel/src/XYThetaInteractiveMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/XYThetaInteractiveMarkerWidget.cpp @@ -5,14 +5,13 @@ namespace mc_rtc_rviz { -XYThetaInteractiveMarkerWidget::XYThetaInteractiveMarkerWidget( - const ClientWidgetParam & params, - const WidgetId & requestId, - std::shared_ptr & server, - const sva::PTransformd & /*pos*/, - bool control_orientation, - bool control_position, - ClientWidget * label) +XYThetaInteractiveMarkerWidget::XYThetaInteractiveMarkerWidget(const ClientWidgetParam & params, + const WidgetId & requestId, + std::shared_ptr & server, + const sva::PTransformd & /*pos*/, + bool control_orientation, + bool control_position, + ClientWidget * label) : InteractiveMarkerWidget(params, requestId, server, makeXYThetaMarker(id2name(requestId)), label) { if(control_position || control_orientation) @@ -39,8 +38,7 @@ void XYThetaInteractiveMarkerWidget::update(const Eigen::Vector3d & vec, double marker_.update(X); } -void XYThetaInteractiveMarkerWidget::handleRequest( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) +void XYThetaInteractiveMarkerWidget::handleRequest(const InteractiveMarkerFeedbackConstPtr & feedback) { auto q = Eigen::Quaterniond{feedback->pose.orientation.w, feedback->pose.orientation.x, feedback->pose.orientation.y, feedback->pose.orientation.z} diff --git a/mc_rtc_rviz_panel/src/XYThetaInteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/XYThetaInteractiveMarkerWidget.h index c2bad4e..4116f78 100644 --- a/mc_rtc_rviz_panel/src/XYThetaInteractiveMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/XYThetaInteractiveMarkerWidget.h @@ -10,13 +10,13 @@ struct XYThetaInteractiveMarkerWidget : public InteractiveMarkerWidget public: XYThetaInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, - std::shared_ptr & server, + std::shared_ptr & server, const sva::PTransformd & pos, bool control_orientation, bool control_position, ClientWidget * label); - void handleRequest(const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback) override; + void handleRequest(const InteractiveMarkerFeedbackConstPtr & feedback) override; // update with X, Y, theta void update(const Eigen::Vector3d & vec, double altitude); @@ -26,8 +26,8 @@ private slots: protected: QCheckBox * coupled_checkbox_; - vm::InteractiveMarker coupled_marker_; - vm::InteractiveMarker decoupled_marker_; + InteractiveMarker coupled_marker_; + InteractiveMarker decoupled_marker_; }; } // namespace mc_rtc_rviz diff --git a/mc_rtc_rviz_panel/src/main.cpp b/mc_rtc_rviz_panel/src/main.cpp index 4d3d2a0..0363d72 100644 --- a/mc_rtc_rviz_panel/src/main.cpp +++ b/mc_rtc_rviz_panel/src/main.cpp @@ -10,6 +10,12 @@ #include #include +#ifdef MC_RTC_ROS_IS_ROS2 +# include +#else +# include +#endif + QApplication * app_ = nullptr; void signal_handler(int) @@ -19,7 +25,11 @@ void signal_handler(int) int main(int argc, char * argv[]) { +#ifdef MC_RTC_ROS_IS_ROS2 + rclcpp::init(argc, argv); +#else ros::init(argc, argv, "mc_rtc_gui"); +#endif QApplication app(argc, argv); app.setWindowIcon(QIcon(":/icons/gui.ico")); app_ = &app; diff --git a/mc_rtc_rviz_panel/src/utils.cpp b/mc_rtc_rviz_panel/src/utils.cpp index 0ed9743..3f71cc0 100644 --- a/mc_rtc_rviz_panel/src/utils.cpp +++ b/mc_rtc_rviz_panel/src/utils.cpp @@ -7,19 +7,19 @@ namespace mc_rtc_rviz { -geometry_msgs::Point rosPoint(const Eigen::Vector3d & vec) +Point rosPoint(const Eigen::Vector3d & vec) { - geometry_msgs::Point p; + Point p; p.x = vec.x(); p.y = vec.y(); p.z = vec.z(); return p; } -vm::Marker makeVisual(int t, double scale) +Marker makeVisual(int t, double scale) { - vm::Marker ret; - ret.action = vm::Marker::ADD; + Marker ret; + ret.action = Marker::ADD; ret.type = t; ret.scale.x = scale; ret.scale.y = scale; @@ -32,16 +32,16 @@ vm::Marker makeVisual(int t, double scale) return ret; } -std::vector makeAxisMarker(double scale) +std::vector makeAxisMarker(double scale) { const Eigen::Vector3d t0 = scale * Eigen::Vector3d{0., 0., 0.}; const Eigen::Vector3d tx = scale * Eigen::Vector3d{1., 0., 0.}; const Eigen::Vector3d ty = scale * Eigen::Vector3d{0., 1., 0.}; const Eigen::Vector3d tz = scale * Eigen::Vector3d{0., 0., 1.}; - vm::Marker m; - m.type = vm::Marker::ARROW; - m.action = vm::Marker::ADD; + Marker m; + m.type = Marker::ARROW; + m.action = Marker::ADD; // Arrow shaft diameter m.scale.x = scale * 0.15; // arrow head diameter @@ -50,7 +50,7 @@ std::vector makeAxisMarker(double scale) m.scale.z = scale * 0.5; m.pose.orientation.w = 1.0; - std::vector ret; + std::vector ret; // X axis m.points.push_back(rosPoint(t0)); m.points.push_back(rosPoint(tx)); @@ -80,14 +80,14 @@ std::vector makeAxisMarker(double scale) return ret; } -std::vector makeArrowMarker(const Eigen::Vector3d & start, - const Eigen::Vector3d & end, - const mc_rtc::gui::ArrowConfig & c) +std::vector makeArrowMarker(const Eigen::Vector3d & start, + const Eigen::Vector3d & end, + const mc_rtc::gui::ArrowConfig & c) { - std::vector markers; - visualization_msgs::Marker m; - m.type = visualization_msgs::Marker::ARROW; - m.action = visualization_msgs::Marker::ADD; + std::vector markers; + Marker m; + m.type = Marker::ARROW; + m.action = Marker::ADD; m.points.push_back(rosPoint(start)); m.points.push_back(rosPoint(end)); m.scale.x = c.shaft_diam; @@ -101,10 +101,9 @@ std::vector makeArrowMarker(const Eigen::Vector3d & start, return markers; } -vm::InteractiveMarkerControl & makeVisualControl(const std::vector & visual_makers, - vm::InteractiveMarker & marker) +InteractiveMarkerControl & makeVisualControl(const std::vector & visual_makers, InteractiveMarker & marker) { - vm::InteractiveMarkerControl ret; + InteractiveMarkerControl ret; ret.always_visible = true; ret.orientation.w = 1.0; ret.markers = visual_makers; @@ -112,12 +111,12 @@ vm::InteractiveMarkerControl & makeVisualControl(const std::vector & return marker.controls.back(); } -vm::InteractiveMarker makeInteractiveMarker(const std::string & name, const std::vector & visual_markers) +InteractiveMarker makeInteractiveMarker(const std::string & name, const std::vector & visual_markers) { - vm::InteractiveMarker marker; + InteractiveMarker marker; marker.header.frame_id = "robot_map"; marker.name = name; - vm::InteractiveMarkerControl ret; + InteractiveMarkerControl ret; ret.always_visible = true; ret.orientation.w = 1.0; ret.markers = visual_markers; @@ -125,25 +124,25 @@ vm::InteractiveMarker makeInteractiveMarker(const std::string & name, const std: return marker; } -vm::InteractiveMarker make6DMarker(const std::string & name, - const std::vector & visual_markers, - bool control_position, - bool control_orientation, - bool move_x, - bool move_y, - bool move_z, - bool rotate_x, - bool rotate_y, - bool rotate_z) +InteractiveMarker make6DMarker(const std::string & name, + const std::vector & visual_markers, + bool control_position, + bool control_orientation, + bool move_x, + bool move_y, + bool move_z, + bool rotate_x, + bool rotate_y, + bool rotate_z) { - vm::InteractiveMarker ret; + InteractiveMarker ret; ret.header.frame_id = "robot_map"; ret.scale = 0.15f; ret.name = name; ret.description = ""; makeVisualControl(visual_markers, ret); - vm::InteractiveMarkerControl control; + InteractiveMarkerControl control; control.orientation.w = 0.707107; control.orientation.x = 0.707107; control.orientation.y = 0; @@ -151,13 +150,13 @@ vm::InteractiveMarker make6DMarker(const std::string & name, if(control_orientation && rotate_x) { control.name = "rotate_x"; - control.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; ret.controls.push_back(control); } if(control_position && move_x) { control.name = "move_x"; - control.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; ret.controls.push_back(control); } @@ -168,13 +167,13 @@ vm::InteractiveMarker make6DMarker(const std::string & name, if(control_orientation && rotate_z) { control.name = "rotate_z"; - control.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; ret.controls.push_back(control); } if(control_position && move_z) { control.name = "move_z"; - control.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; ret.controls.push_back(control); } @@ -185,32 +184,32 @@ vm::InteractiveMarker make6DMarker(const std::string & name, if(control_orientation && rotate_y) { control.name = "rotate_y"; - control.interaction_mode = vm::InteractiveMarkerControl::ROTATE_AXIS; + control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS; ret.controls.push_back(control); } if(control_position && move_y) { control.name = "move_y"; - control.interaction_mode = vm::InteractiveMarkerControl::MOVE_AXIS; + control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS; ret.controls.push_back(control); } return ret; } -vm::InteractiveMarker make3DMarker(const std::string & name, - const std::vector & visual_markers, - bool control_position, - bool /*move_x*/, - bool /*move_y*/, - bool /*move_z*/) +InteractiveMarker make3DMarker(const std::string & name, + const std::vector & visual_markers, + bool control_position, + bool /*move_x*/, + bool /*move_y*/, + bool /*move_z*/) { return make6DMarker(name, visual_markers, control_position, false); } -vm::InteractiveMarker makeXYThetaMarker(const std::string & name, bool readonly) +InteractiveMarker makeXYThetaMarker(const std::string & name, bool readonly) { - vm::InteractiveMarker int_marker; + InteractiveMarker int_marker; int_marker.header.frame_id = "robot_map"; int_marker.scale = 0.25; int_marker.name = name; @@ -219,24 +218,22 @@ vm::InteractiveMarker makeXYThetaMarker(const std::string & name, bool readonly) if(!readonly) { - vm::InteractiveMarkerControl control; + InteractiveMarkerControl control; control.orientation.w = 1; control.orientation.x = 0; control.orientation.y = 1; control.orientation.z = 0; - control.interaction_mode = vm::InteractiveMarkerControl::MOVE_ROTATE; + control.interaction_mode = InteractiveMarkerControl::MOVE_ROTATE; int_marker.controls.push_back(control); } return int_marker; } -visualization_msgs::Marker getPointMarker(const Eigen::Vector3d & pos, - const mc_rtc::gui::Color & color, - double scale = 0.02) +Marker getPointMarker(const Eigen::Vector3d & pos, const mc_rtc::gui::Color & color, double scale = 0.02) { - visualization_msgs::Marker m; - m.type = visualization_msgs::Marker::SPHERE; - m.action = visualization_msgs::Marker::ADD; + Marker m; + m.type = Marker::SPHERE; + m.action = Marker::ADD; m.scale.x = scale; m.scale.y = scale; m.scale.z = scale; @@ -254,10 +251,10 @@ visualization_msgs::Marker getPointMarker(const Eigen::Vector3d & pos, return m; } -SharedMarker::SharedMarker(std::shared_ptr server, +SharedMarker::SharedMarker(std::shared_ptr server, const std::string & /*name*/, - const vm::InteractiveMarker & marker, - interactive_markers::InteractiveMarkerServer::FeedbackCallback callback) + const InteractiveMarker & marker, + InteractiveMarkerServer::FeedbackCallback callback) : server_(server), marker_(marker), callback_(callback) { server_->insert(marker_, callback_); @@ -286,7 +283,7 @@ void SharedMarker::update(const Eigen::Vector3d & t) { if(!hidden_) { - geometry_msgs::Pose pose; + Pose pose; pose.orientation.w = 1.0; pose.position.x = t.x(); pose.position.y = t.y(); @@ -299,7 +296,7 @@ void SharedMarker::update(const sva::PTransformd & pos) { if(!hidden_) { - geometry_msgs::Pose pose; + Pose pose; Eigen::Quaterniond q{pos.rotation().transpose()}; pose.orientation.w = q.w(); pose.orientation.x = q.x(); diff --git a/mc_rtc_rviz_panel/src/utils.h b/mc_rtc_rviz_panel/src/utils.h index 3bb4f82..550a870 100644 --- a/mc_rtc_rviz_panel/src/utils.h +++ b/mc_rtc_rviz_panel/src/utils.h @@ -5,17 +5,45 @@ #pragma once #include +#include #include -#include -#include -#include -namespace vm = visualization_msgs; +#ifdef MC_RTC_ROS_IS_ROS2 +# include +# include +# include +# include +#else +# include +# include +# include +# include +#endif namespace mc_rtc_rviz { +#ifdef MC_RTC_ROS_IS_ROS2 +using Marker = visualization_msgs::msg::Marker; +using MarkerArray = visualization_msgs::msg::MarkerArray; +using Point = geometry_msgs::msg::Point; +using InteractiveMarker = visualization_msgs::msg::InteractiveMarker; +using InteractiveMarkerServer = interactive_markers::InteractiveMarkerServer; +using InteractiveMarkerControl = visualization_msgs::msg::InteractiveMarkerControl; +using InteractiveMarkerFeedbackConstPtr = visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr; +using Pose = geometry_msgs::msg::Pose; +#else +using Marker = visualization_msgs::Marker; +using MarkerArray = visualization_msgs::MarkerArray; +using Point = geometry_msgs::Point; +using InteractiveMarker = visualization_msgs::InteractiveMarker; +using InteractiveMarkerServer = interactive_markers::InteractiveMarkerServer; +using InteractiveMarkerControl = visualization_msgs::InteractiveMarkerControl; +using InteractiveMarkerFeedbackConstPtr = visualization_msgs::InteractiveMarkerFeedbackConstPtr; +using Pose = geometry_msgs::Pose; +#endif + // Check if at least one values in x.array() is nan template inline bool is_nan(const Eigen::MatrixBase & x) @@ -29,37 +57,37 @@ inline bool is_in_range(const Eigen::MatrixBase & x, double min = -10e1 return (x.array() > min).all() && (x.array() < max).all(); } -visualization_msgs::Marker getPointMarker(const Eigen::Vector3d & pos, const mc_rtc::gui::Color & color, double scale); -geometry_msgs::Point rosPoint(const Eigen::Vector3d & vec); -vm::Marker makeVisual(int t, double baseScale); -vm::InteractiveMarker makeInteractiveMarker(const std::string & name, const std::vector & visual_markers); -std::vector makeAxisMarker(double baseScale); -vm::InteractiveMarker make6DMarker(const std::string & name, - const std::vector & visual_markers, - bool control_position, - bool control_orientation, - bool move_x = true, - bool move_y = true, - bool move_z = true, - bool rotate_x = true, - bool rotate_y = true, - bool rotate_z = true); -vm::InteractiveMarker make3DMarker(const std::string & name, - const std::vector & visual_markers, - bool control_position, - bool move_x = true, - bool move_y = true, - bool move_z = true); -vm::InteractiveMarker makeXYThetaMarker(const std::string & name, bool readonly = false); -std::vector makeArrowMarker(const Eigen::Vector3d & start, - const Eigen::Vector3d & end, - const mc_rtc::gui::ArrowConfig & c); +Marker getPointMarker(const Eigen::Vector3d & pos, const mc_rtc::gui::Color & color, double scale); +Point rosPoint(const Eigen::Vector3d & vec); +Marker makeVisual(int t, double baseScale); +InteractiveMarker makeInteractiveMarker(const std::string & name, const std::vector & visual_markers); +std::vector makeAxisMarker(double baseScale); +InteractiveMarker make6DMarker(const std::string & name, + const std::vector & visual_markers, + bool control_position, + bool control_orientation, + bool move_x = true, + bool move_y = true, + bool move_z = true, + bool rotate_x = true, + bool rotate_y = true, + bool rotate_z = true); +InteractiveMarker make3DMarker(const std::string & name, + const std::vector & visual_markers, + bool control_position, + bool move_x = true, + bool move_y = true, + bool move_z = true); +InteractiveMarker makeXYThetaMarker(const std::string & name, bool readonly = false); +std::vector makeArrowMarker(const Eigen::Vector3d & start, + const Eigen::Vector3d & end, + const mc_rtc::gui::ArrowConfig & c); struct SharedMarker { SharedMarker(std::shared_ptr server, const std::string & name, - const vm::InteractiveMarker & marker, + const InteractiveMarker & marker, interactive_markers::InteractiveMarkerServer::FeedbackCallback callback); ~SharedMarker(); @@ -70,21 +98,21 @@ struct SharedMarker void update(const sva::PTransformd & pos); - void marker(const vm::InteractiveMarker & marker) + void marker(const InteractiveMarker & marker) { server_->erase(marker_.name); marker_ = marker; server_->insert(marker_, callback_); } - vm::InteractiveMarker & marker() { return marker_; } + InteractiveMarker & marker() { return marker_; } void applyChanges() { server_->applyChanges(); } private: - std::shared_ptr server_; - vm::InteractiveMarker marker_; - interactive_markers::InteractiveMarkerServer::FeedbackCallback callback_; + std::shared_ptr server_; + InteractiveMarker marker_; + InteractiveMarkerServer::FeedbackCallback callback_; bool hidden_ = false; }; diff --git a/mc_rtc_ticker/CMakeLists.txt b/mc_rtc_ticker/CMakeLists.txt index 5ade407..6e38657 100644 --- a/mc_rtc_ticker/CMakeLists.txt +++ b/mc_rtc_ticker/CMakeLists.txt @@ -10,7 +10,29 @@ project( LANGUAGES CXX VERSION 1.5.0) -find_package(catkin REQUIRED) -catkin_package() +if(NOT DEFINED ENV{ROS_VERSION}) + message(FATAL_ERROR "Expected to find ROS_VERSION in the environment") +endif() +set(ROS_VERSION $ENV{ROS_VERSION}) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +if(ROS_VERSION EQUAL 1) + find_package(catkin REQUIRED) + catkin_package() + + install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +elseif(ROS_VERSION EQUAL 2) + find_package(ament_cmake REQUIRED) + ament_package() + + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + install( + FILES launch/display.2.launch + DESTINATION share/${PROJECT_NAME}/launch/ + RENAME display.launch) + install( + FILES launch/display.2.rviz + DESTINATION share/${PROJECT_NAME}/launch/ + RENAME display.rviz) +else() + message(FATAL_ERROR "ROS_VERSION ${ROS_VERSION} is not supported yet") +endif() diff --git a/mc_rtc_ticker/launch/display.2.launch b/mc_rtc_ticker/launch/display.2.launch new file mode 100644 index 0000000..b9c7e09 --- /dev/null +++ b/mc_rtc_ticker/launch/display.2.launch @@ -0,0 +1,3 @@ + + + diff --git a/mc_rtc_ticker/launch/display.2.rviz b/mc_rtc_ticker/launch/display.2.rviz new file mode 100644 index 0000000..864eec2 --- /dev/null +++ b/mc_rtc_ticker/launch/display.2.rviz @@ -0,0 +1,492 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 380 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: mc_rtc_rviz_panel/mc_rtc panel + Name: mc_rtc panel +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + L_ANKLE_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_ANKLE_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + L_ELBOW_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_ELBOW_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_HIP_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + L_HIP_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_HIP_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_KNEE_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_LINDEX_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_LLITTLE_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_LTHUMB_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_SHOULDER_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_SHOULDER_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + L_SHOULDER_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_UINDEX_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_ULITTLE_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_UTHUMB_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + L_WRIST_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + L_WRIST_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + NECK_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + NECK_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + NECK_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + PELVIS_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_ANKLE_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_ANKLE_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + R_ELBOW_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_ELBOW_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_HIP_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + R_HIP_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_HIP_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_KNEE_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_LINDEX_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_LLITTLE_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_LTHUMB_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_SHOULDER_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_SHOULDER_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + R_SHOULDER_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_UINDEX_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_ULITTLE_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_UTHUMB_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + R_WRIST_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + R_WRIST_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WAIST_P_S: + Alpha: 1 + Show Axes: false + Show Trail: false + WAIST_R_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WAIST_Y_S: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + dcamera: + Alpha: 1 + Show Axes: false + Show Trail: false + gsensor: + Alpha: 1 + Show Axes: false + Show Trail: false + gyrometer: + Alpha: 1 + Show Axes: false + Show Trail: false + l_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + l_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + lcamera: + Alpha: 1 + Show Axes: false + Show Trail: false + lfsensor: + Alpha: 1 + Show Axes: false + Show Trail: false + lhsensor: + Alpha: 1 + Show Axes: false + Show Trail: false + r_ankle: + Alpha: 1 + Show Axes: false + Show Trail: false + r_wrist: + Alpha: 1 + Show Axes: false + Show Trail: false + ranger: + Alpha: 1 + Show Axes: false + Show Trail: false + rcamera: + Alpha: 1 + Show Axes: false + Show Trail: false + rfsensor: + Alpha: 1 + Show Axes: false + Show Trail: false + rhsensor: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: MainRobot + TF Prefix: control + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/env_1/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ground: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: Env + TF Prefix: control/env_1 + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /mc_rtc_rviz_interactive_markers + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mc_rtc_rviz + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: robot_map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.4216043949127197 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.2895575761795044 + Y: 0.1488959789276123 + Z: 0.9942084550857544 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4412866234779358 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7535669803619385 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1030 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000022a00000373fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001b7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018006d0063005f007200740063002000700061006e0065006c01000001f8000001b60000007100ffffff000000010000010000000373fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000373000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000037fc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005500000037300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 0 + Y: 26 + mc_rtc panel: + collapsed: false From c895a33f0e2ecc1bdff53c4d76a25f6aabf0834c Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Fri, 16 Feb 2024 11:42:06 +0900 Subject: [PATCH 07/10] Adapt for ROS2 --- mc_rtc_rviz_panel/CMakeLists.txt | 6 ++- .../src/ArrowInteractiveMarkerWidget.cpp | 20 +++++---- .../src/ArrowInteractiveMarkerWidget.h | 4 +- mc_rtc_rviz_panel/src/CMakeLists.txt | 12 +++--- mc_rtc_rviz_panel/src/ClientWidget.cpp | 7 +++- mc_rtc_rviz_panel/src/ClientWidget.h | 15 +++++++ .../src/DisplayTrajectoryWidget.cpp | 10 ++--- .../src/ForceInteractiveMarkerWidget.cpp | 2 + .../src/ForceInteractiveMarkerWidget.h | 1 + mc_rtc_rviz_panel/src/Panel.cpp | 42 +++++++++++++++---- mc_rtc_rviz_panel/src/Panel.h | 3 ++ mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp | 4 +- .../src/PolyhedronMarkerWidget.cpp | 10 ++--- mc_rtc_rviz_panel/src/VisualWidget.cpp | 9 +++- mc_rtc_rviz_panel/src/plugin.cpp | 9 +++- mc_rtc_rviz_panel/src/plugin.h | 14 ++++++- mc_rtc_rviz_panel/src/utils.cpp | 8 +--- mc_rtc_rviz_panel/src/utils.h | 20 +++++---- mc_rtc_ticker/CMakeLists.txt | 4 +- 19 files changed, 139 insertions(+), 61 deletions(-) diff --git a/mc_rtc_rviz_panel/CMakeLists.txt b/mc_rtc_rviz_panel/CMakeLists.txt index 47aaa78..d2fe953 100644 --- a/mc_rtc_rviz_panel/CMakeLists.txt +++ b/mc_rtc_rviz_panel/CMakeLists.txt @@ -56,10 +56,12 @@ elseif(ROS_VERSION EQUAL 2) add_ros2_dependency(rviz_common rviz_common) pluginlib_export_plugin_description_file(rviz_common plugin/rviz_plugin.xml) - - ament_package() else() message(FATAL_ERROR "ROS_VERSION ${ROS_VERSION} is not supported yet") endif() add_subdirectory(src) + +if(ROS_VERSION EQUAL 2) + ament_package() +endif() diff --git a/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.cpp b/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.cpp index b9f7124..c6e8e6c 100644 --- a/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.cpp @@ -6,13 +6,14 @@ namespace mc_rtc_rviz ArrowInteractiveMarkerWidget::ArrowInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, std::shared_ptr & server, + MarkerArray & markers, const Eigen::Vector3d & start, const Eigen::Vector3d & end, const mc_rtc::gui::ArrowConfig & config, bool control_start, bool control_end, ClientWidget * label) -: ClientWidget(params), request_id_(requestId), +: ClientWidget(params), request_id_(requestId), markers_(markers), start_marker_(server, id2name(requestId), InteractiveMarker{}, @@ -20,11 +21,7 @@ ArrowInteractiveMarkerWidget::ArrowInteractiveMarkerWidget(const ClientWidgetPar end_marker_(server, id2name(requestId), InteractiveMarker{}, - [this](const InteractiveMarkerFeedbackConstPtr & feedback) { handleEndRequest(feedback); }), - arrow_marker_(server, - id2name(requestId), - makeInteractiveMarker(id2name(requestId) + "_arrow", makeArrowMarker(start, end, config)), - [](const InteractiveMarkerFeedbackConstPtr &) {}) + [this](const InteractiveMarkerFeedbackConstPtr & feedback) { handleEndRequest(feedback); }) { if(config.start_point_scale > 0) @@ -45,6 +42,10 @@ ArrowInteractiveMarkerWidget::ArrowInteractiveMarkerWidget(const ClientWidgetPar start_marker_.applyChanges(); end_marker_.applyChanges(); + arrow_marker_ = makeArrowMarker(start, end, config); + arrow_marker_.ns = id2name(params.id) + "_arrow"; + arrow_marker_.header.frame_id = "robot_map"; + arrow_points_ << start, end; button_ = label->showHideButton(); button_->setCheckable(true); @@ -76,8 +77,10 @@ void ArrowInteractiveMarkerWidget::update(const Eigen::Vector3d & start, { start_marker_.update(start); end_marker_.update(end); - arrow_marker_.marker(makeInteractiveMarker(id2name(request_id_) + "_arrow", makeArrowMarker(start, end, c))); - arrow_marker_.applyChanges(); + arrow_marker_.header.stamp = now(); + arrow_marker_.points[0] = rosPoint(start); + arrow_marker_.points[1] = rosPoint(end); + markers_.markers.push_back(arrow_marker_); } } @@ -99,7 +102,6 @@ void ArrowInteractiveMarkerWidget::toggled(bool hide) { start_marker_.toggle(); end_marker_.toggle(); - arrow_marker_.toggle(); button_->setText(hide ? "Show" : "Hide"); visible(!hide); } diff --git a/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.h index dee2152..57890a6 100644 --- a/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/ArrowInteractiveMarkerWidget.h @@ -14,6 +14,7 @@ struct ArrowInteractiveMarkerWidget : public ClientWidget ArrowInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, std::shared_ptr & server, + MarkerArray & markers, const Eigen::Vector3d & start, const Eigen::Vector3d & end, const mc_rtc::gui::ArrowConfig & config, @@ -29,10 +30,11 @@ struct ArrowInteractiveMarkerWidget : public ClientWidget protected: WidgetId request_id_; + MarkerArray & markers_; Eigen::Vector6d arrow_points_; SharedMarker start_marker_; SharedMarker end_marker_; - SharedMarker arrow_marker_; + Marker arrow_marker_; QPushButton * button_; private slots: diff --git a/mc_rtc_rviz_panel/src/CMakeLists.txt b/mc_rtc_rviz_panel/src/CMakeLists.txt index 690c079..cafceed 100644 --- a/mc_rtc_rviz_panel/src/CMakeLists.txt +++ b/mc_rtc_rviz_panel/src/CMakeLists.txt @@ -128,13 +128,15 @@ endif() target_compile_options(_gui_objects PRIVATE -fPIC) target_link_libraries(_gui_objects PUBLIC Qwt::Qwt mc_rtc::mc_control_client mc_rtc::mc_rtc_ros) - -add_library(${PROJECT_NAME} Panel.cpp ${HEADER_FILES} ${MOC_HEADERS}) if(ROS_VERSION EQUAL 1) - add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) + add_dependencies(_gui_objects ${catkin_EXPORTED_TARGETS}) + target_link_libraries(_gui_objects PUBLIC ${catkin_LIBRARIES}) elseif(ROS_VERSION EQUAL 2) - target_link_libraries(${PROJECT_NAME} PUBLIC ROSDependencies) + target_link_libraries(_gui_objects PUBLIC ROSDependencies) +endif() + +add_library(${PROJECT_NAME} SHARED Panel.cpp ${HEADER_FILES} ${MOC_HEADERS}) +if(ROS_VERSION EQUAL 2) target_compile_definitions( ${PROJECT_NAME} PRIVATE RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY MC_RTC_RVIZ_PANEL_IS_ROS2) diff --git a/mc_rtc_rviz_panel/src/ClientWidget.cpp b/mc_rtc_rviz_panel/src/ClientWidget.cpp index 6b974d4..9716749 100644 --- a/mc_rtc_rviz_panel/src/ClientWidget.cpp +++ b/mc_rtc_rviz_panel/src/ClientWidget.cpp @@ -12,7 +12,7 @@ namespace mc_rtc_rviz std::string id2name(const WidgetId & id) { std::string ret; - for(auto & c : id.category) { ret += c + "/"; } + for(const auto & c : id.category) { ret += c + "/"; } ret += id.name; return ret; } @@ -42,4 +42,9 @@ void ClientWidget::visible(bool visibility) static_cast(client_).visible(id_, visibility); } +Time ClientWidget::now() const +{ + return dynamic_cast(client_).now(); +} + } // namespace mc_rtc_rviz diff --git a/mc_rtc_rviz_panel/src/ClientWidget.h b/mc_rtc_rviz_panel/src/ClientWidget.h index 2f06f8a..f846ed9 100644 --- a/mc_rtc_rviz_panel/src/ClientWidget.h +++ b/mc_rtc_rviz_panel/src/ClientWidget.h @@ -8,11 +8,23 @@ #include +#ifdef MC_RTC_ROS_IS_ROS2 +# include +#else +# include +#endif + namespace mc_rtc_rviz { using WidgetId = mc_control::ElementId; +#ifdef MC_RTC_ROS_IS_ROS2 +using Time = rclcpp::Time; +#else +using Time = ros::Time; +#endif + struct ClientWidgetParam { mc_control::ControllerClient & client; @@ -75,6 +87,9 @@ struct ClientWidget : public QWidget /** To be implemented in container, return a widget by its name or a nullptr if the widget does not exist */ virtual ClientWidget * widget(const std::string &) { return nullptr; } + /** Get the current ROS time */ + Time now() const; + protected: mc_control::ControllerClient & client() { return client_; } diff --git a/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.cpp b/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.cpp index d6311b2..c2a9a71 100644 --- a/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.cpp +++ b/mc_rtc_rviz_panel/src/DisplayTrajectoryWidget.cpp @@ -41,7 +41,7 @@ void DisplayTrajectoryWidget::update(const std::vector & points p.transpose()); return; } - geometry_msgs::Point pose; + Point pose; pose.x = p.x(); pose.y = p.y(); pose.z = p.z(); @@ -64,7 +64,7 @@ void DisplayTrajectoryWidget::update(const std::vector & point p.transpose()); return; } - geometry_msgs::Point pose; + Point pose; pose.x = p.x(); pose.y = p.y(); pose.z = p.z(); @@ -82,7 +82,7 @@ void DisplayTrajectoryWidget::update(const Eigen::Vector3d & point, const mc_rtc point.transpose()); return; } - geometry_msgs::Point pose; + Point pose; pose.x = point.x(); pose.y = point.y(); pose.z = point.z(); @@ -101,7 +101,7 @@ void DisplayTrajectoryWidget::update(const sva::PTransformd & point, const mc_rt return; } configure(config); - geometry_msgs::Point pose; + Point pose; pose.x = p.x(); pose.y = p.y(); pose.z = p.z(); @@ -114,7 +114,7 @@ void DisplayTrajectoryWidget::configure(const mc_rtc::gui::LineConfig & config) { path_.type = config.style == mc_rtc::gui::LineStyle::Dotted ? Marker::POINTS : Marker::LINE_STRIP; path_.header.frame_id = "robot_map"; - path_.header.stamp = ros::Time::now(); + path_.header.stamp = now(); path_.pose.orientation.w = 1.0; path_.pose.orientation.x = 0.0; path_.pose.orientation.y = 0.0; diff --git a/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.cpp b/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.cpp index ed0a478..de7d700 100644 --- a/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.cpp @@ -6,6 +6,7 @@ namespace mc_rtc_rviz ForceInteractiveMarkerWidget::ForceInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, std::shared_ptr & server, + MarkerArray & markers, const sva::PTransformd & surface, const sva::ForceVecd & force, const mc_rtc::gui::ForceConfig & config, @@ -14,6 +15,7 @@ ForceInteractiveMarkerWidget::ForceInteractiveMarkerWidget(const ClientWidgetPar : ArrowInteractiveMarkerWidget(params, requestId, server, + markers, surface.translation(), surface.translation(), config, diff --git a/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.h b/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.h index 256cdc3..aa3472e 100644 --- a/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.h +++ b/mc_rtc_rviz_panel/src/ForceInteractiveMarkerWidget.h @@ -13,6 +13,7 @@ struct ForceInteractiveMarkerWidget : public ArrowInteractiveMarkerWidget ForceInteractiveMarkerWidget(const ClientWidgetParam & params, const WidgetId & requestId, std::shared_ptr & server, + MarkerArray & markers, const sva::PTransformd & surface, const sva::ForceVecd & force, const mc_rtc::gui::ForceConfig & config, diff --git a/mc_rtc_rviz_panel/src/Panel.cpp b/mc_rtc_rviz_panel/src/Panel.cpp index a935680..f4ef073 100644 --- a/mc_rtc_rviz_panel/src/Panel.cpp +++ b/mc_rtc_rviz_panel/src/Panel.cpp @@ -112,14 +112,26 @@ double getTimeout() struct PanelImpl { - PanelImpl() : nh_(), int_server_(std::make_shared("mc_rtc_rviz_interactive_markers")) + PanelImpl() { - marker_array_pub_ = mc_rtc::ROSBridge::get_node_handle()->advertise("/mc_rtc_rviz", 0); +#ifdef MC_RTC_ROS_IS_ROS2 + nh_ = rclcpp::Node::make_shared("mc_rtc_rviz_panel"); + marker_array_pub_ = nh_->create_publisher("/mc_rtc_rviz", 0); + int_server_ = std::make_shared("mc_rtc_rviz_interactive_markers", nh_); +#else + int_server_ = std::make_shared("mc_rtc_rviz_interactive_markers"); + marker_array_pub_ = nh_.advertise("/mc_rtc_rviz", 0); +#endif } +#ifdef MC_RTC_ROS_IS_ROS2 + mc_rtc::NodeHandlePtr nh_; + rclcpp::Publisher::SharedPtr marker_array_pub_; +#else ros::NodeHandle nh_; + ros::Publisher marker_array_pub_; +#endif std::shared_ptr int_server_; MarkerArray marker_array_; - ros::Publisher marker_array_pub_; }; Panel::Panel(QWidget * parent) @@ -269,9 +281,14 @@ void Panel::got_stop() tree_.clean(); this->clean(); impl_->int_server_->applyChanges(); +#ifdef MC_RTC_ROS_IS_ROS2 + impl_->marker_array_pub_->publish(impl_->marker_array_); + if(rclcpp::ok()) { rclcpp::spin_some(impl_->nh_); } +#else impl_->marker_array_pub_.publish(impl_->marker_array_); - impl_->marker_array_.markers.clear(); if(ros::ok()) { ros::spinOnce(); } +#endif + impl_->marker_array_.markers.clear(); } Panel::~Panel() @@ -825,8 +842,8 @@ void Panel::got_force(const WidgetId & id, bool ro) { auto label = latestWidget_; - auto & w = get_widget(id, requestId, impl_->int_server_, surface, forcep, forceConfig, - ro, label); + auto & w = get_widget(id, requestId, impl_->int_server_, impl_->marker_array_, surface, + forcep, forceConfig, ro, label); w.update(surface, forcep, forceConfig); } @@ -838,8 +855,8 @@ void Panel::got_arrow(const WidgetId & id, bool ro) { auto label = latestWidget_; - auto & w = - get_widget(id, requestId, impl_->int_server_, start, end, config, !ro, !ro, label); + auto & w = get_widget(id, requestId, impl_->int_server_, impl_->marker_array_, start, + end, config, !ro, !ro, label); w.update(start, end, config); } @@ -1194,4 +1211,13 @@ mc_rtc::Configuration Panel::config(const WidgetId & id) const return ret(id.name); } +Time Panel::now() const +{ +#ifdef MC_RTC_ROS_IS_ROS2 + return impl_->nh_->now(); +#else + return ros::Time::now(); +#endif +} + } // namespace mc_rtc_rviz diff --git a/mc_rtc_rviz_panel/src/Panel.h b/mc_rtc_rviz_panel/src/Panel.h index 82963c1..8806e04 100644 --- a/mc_rtc_rviz_panel/src/Panel.h +++ b/mc_rtc_rviz_panel/src/Panel.h @@ -34,6 +34,9 @@ class Panel : public CategoryWidget, public mc_control::ControllerClient using CategoryWidget::connect; + /** Returns the current ROS time */ + Time now() const; + /** Returns true if the provided widget should be visible * * If the widget has never been seen, returns true diff --git a/mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp b/mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp index cf7ccda..ab2abf7 100644 --- a/mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/PolygonMarkerWidget.cpp @@ -70,7 +70,7 @@ void PolygonMarkerWidget::update(const std::string & ns, mc_rtc::log::error("Could not display polygon {}: invalid value in coordinates ({})", ns, point.transpose()); return; } - geometry_msgs::Point p; + Point p; p.x = point.x(); p.y = point.y(); p.z = point.z(); @@ -82,7 +82,7 @@ void PolygonMarkerWidget::update(const std::string & ns, m.color.g = static_cast(c.color.g); m.color.b = static_cast(c.color.b); m.color.a = static_cast(c.color.a); - m.header.stamp = ros::Time::now(); + m.header.stamp = now(); m.header.frame_id = "robot_map"; m.ns = ns; m.id = static_cast(id); diff --git a/mc_rtc_rviz_panel/src/PolyhedronMarkerWidget.cpp b/mc_rtc_rviz_panel/src/PolyhedronMarkerWidget.cpp index a0bb2dc..7db410c 100644 --- a/mc_rtc_rviz_panel/src/PolyhedronMarkerWidget.cpp +++ b/mc_rtc_rviz_panel/src/PolyhedronMarkerWidget.cpp @@ -94,7 +94,7 @@ void PolyhedronMarkerWidget::update_triangles(const std::vector +#ifdef MC_RTC_ROS_IS_ROS2 +# include +PLUGINLIB_EXPORT_CLASS(mc_rtc_rviz::MyPanel, rviz_common::Panel) +#else +# include PLUGINLIB_EXPORT_CLASS(mc_rtc_rviz::MyPanel, rviz::Panel) +#endif diff --git a/mc_rtc_rviz_panel/src/plugin.h b/mc_rtc_rviz_panel/src/plugin.h index 4969374..f348cec 100644 --- a/mc_rtc_rviz_panel/src/plugin.h +++ b/mc_rtc_rviz_panel/src/plugin.h @@ -2,20 +2,30 @@ * Copyright 2016-2019 CNRS-UM LIRMM, CNRS-AIST JRL */ -#include +#ifdef MC_RTC_ROS_IS_ROS2 +# include +using PanelBase = rviz_common::Panel; +#else +# include +using PanelBase = rviz::Panel; +#endif #include "Panel.h" namespace mc_rtc_rviz { -class MyPanel : public rviz::Panel +class MyPanel : public PanelBase { Q_OBJECT public: MyPanel(QWidget * parent = 0); +#ifdef MC_RTC_ROS_IS_ROS2 + virtual ~MyPanel(); +#else virtual ~MyPanel() override; +#endif mc_rtc_rviz::Panel * panel; }; diff --git a/mc_rtc_rviz_panel/src/utils.cpp b/mc_rtc_rviz_panel/src/utils.cpp index 3f71cc0..f0c276f 100644 --- a/mc_rtc_rviz_panel/src/utils.cpp +++ b/mc_rtc_rviz_panel/src/utils.cpp @@ -80,11 +80,8 @@ std::vector makeAxisMarker(double scale) return ret; } -std::vector makeArrowMarker(const Eigen::Vector3d & start, - const Eigen::Vector3d & end, - const mc_rtc::gui::ArrowConfig & c) +Marker makeArrowMarker(const Eigen::Vector3d & start, const Eigen::Vector3d & end, const mc_rtc::gui::ArrowConfig & c) { - std::vector markers; Marker m; m.type = Marker::ARROW; m.action = Marker::ADD; @@ -97,8 +94,7 @@ std::vector makeArrowMarker(const Eigen::Vector3d & start, m.color.r = static_cast(c.color.r); m.color.g = static_cast(c.color.g); m.color.b = static_cast(c.color.b); - markers.push_back(m); - return markers; + return m; } InteractiveMarkerControl & makeVisualControl(const std::vector & visual_makers, InteractiveMarker & marker) diff --git a/mc_rtc_rviz_panel/src/utils.h b/mc_rtc_rviz_panel/src/utils.h index 550a870..b0e7802 100644 --- a/mc_rtc_rviz_panel/src/utils.h +++ b/mc_rtc_rviz_panel/src/utils.h @@ -10,11 +10,13 @@ #include #ifdef MC_RTC_ROS_IS_ROS2 +# include # include # include # include # include #else +# include # include # include # include @@ -25,22 +27,24 @@ namespace mc_rtc_rviz { #ifdef MC_RTC_ROS_IS_ROS2 -using Marker = visualization_msgs::msg::Marker; -using MarkerArray = visualization_msgs::msg::MarkerArray; -using Point = geometry_msgs::msg::Point; +using ColorRGBA = std_msgs::msg::ColorRGBA; using InteractiveMarker = visualization_msgs::msg::InteractiveMarker; using InteractiveMarkerServer = interactive_markers::InteractiveMarkerServer; using InteractiveMarkerControl = visualization_msgs::msg::InteractiveMarkerControl; using InteractiveMarkerFeedbackConstPtr = visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr; +using Marker = visualization_msgs::msg::Marker; +using MarkerArray = visualization_msgs::msg::MarkerArray; +using Point = geometry_msgs::msg::Point; using Pose = geometry_msgs::msg::Pose; #else -using Marker = visualization_msgs::Marker; -using MarkerArray = visualization_msgs::MarkerArray; -using Point = geometry_msgs::Point; +using ColorRGBA = std_msgs::ColorRGBA; using InteractiveMarker = visualization_msgs::InteractiveMarker; using InteractiveMarkerServer = interactive_markers::InteractiveMarkerServer; using InteractiveMarkerControl = visualization_msgs::InteractiveMarkerControl; using InteractiveMarkerFeedbackConstPtr = visualization_msgs::InteractiveMarkerFeedbackConstPtr; +using Marker = visualization_msgs::Marker; +using MarkerArray = visualization_msgs::MarkerArray; +using Point = geometry_msgs::Point; using Pose = geometry_msgs::Pose; #endif @@ -79,9 +83,7 @@ InteractiveMarker make3DMarker(const std::string & name, bool move_y = true, bool move_z = true); InteractiveMarker makeXYThetaMarker(const std::string & name, bool readonly = false); -std::vector makeArrowMarker(const Eigen::Vector3d & start, - const Eigen::Vector3d & end, - const mc_rtc::gui::ArrowConfig & c); +Marker makeArrowMarker(const Eigen::Vector3d & start, const Eigen::Vector3d & end, const mc_rtc::gui::ArrowConfig & c); struct SharedMarker { diff --git a/mc_rtc_ticker/CMakeLists.txt b/mc_rtc_ticker/CMakeLists.txt index 6e38657..34e07e9 100644 --- a/mc_rtc_ticker/CMakeLists.txt +++ b/mc_rtc_ticker/CMakeLists.txt @@ -22,9 +22,7 @@ if(ROS_VERSION EQUAL 1) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) elseif(ROS_VERSION EQUAL 2) find_package(ament_cmake REQUIRED) - ament_package() - install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install( FILES launch/display.2.launch DESTINATION share/${PROJECT_NAME}/launch/ @@ -33,6 +31,8 @@ elseif(ROS_VERSION EQUAL 2) FILES launch/display.2.rviz DESTINATION share/${PROJECT_NAME}/launch/ RENAME display.rviz) + + ament_package() else() message(FATAL_ERROR "ROS_VERSION ${ROS_VERSION} is not supported yet") endif() From a9256a5c65c423f91cfca6e05ab104b701d62b4f Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Fri, 16 Feb 2024 17:18:13 +0900 Subject: [PATCH 08/10] [ci] Add colcon build on Jammy --- .github/workflows/build.yml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 72464c2..c7c98a6 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -19,7 +19,7 @@ jobs: strategy: fail-fast: false matrix: - os: [ubuntu-20.04] + os: [ubuntu-22.04, ubuntu-20.04] runs-on: ${{ matrix.os }} steps: - name: Install dependencies @@ -29,12 +29,15 @@ jobs: apt-mirrors: mc-rtc: cloudsmith: mc-rtc/head - apt: libmc-rtc-dev qt5-default libqwt-qt5-dev + apt: libmc-rtc-dev libqwt-qt5-dev ros: | apt: mc-rtc-plugin - name: Build with catkin if: ${{ matrix.os == 'ubuntu-20.04' }} uses: jrl-umi3218/github-actions/build-catkin-project@master + - name: Build with colcon + if: ${{ matrix.os != 'ubuntu-20.04' }} + uses: jrl-umi3218/github-actions/build-colcon-project@master - name: Slack Notification if: failure() uses: archive/github-actions-slack@master From 0a78e1614b57d61e0663b7b60627d1733c115bf2 Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Fri, 16 Feb 2024 18:07:54 +0900 Subject: [PATCH 09/10] [ci] Install qt5-default on 20.04 --- .github/workflows/build.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index c7c98a6..9aa20a2 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -32,6 +32,12 @@ jobs: apt: libmc-rtc-dev libqwt-qt5-dev ros: | apt: mc-rtc-plugin + - name: Install Qt5 + if: ${{ matrix.os == 'ubuntu-20.04' }} + uses: jrl-umi3218/github-actions/install-dependencies@master + with: + ubuntu: | + apt: qt5-default - name: Build with catkin if: ${{ matrix.os == 'ubuntu-20.04' }} uses: jrl-umi3218/github-actions/build-catkin-project@master From 5c4be17fc78b8edcc0cccc01b177a2d7825534eb Mon Sep 17 00:00:00 2001 From: Pierre Gergondet Date: Fri, 16 Feb 2024 18:33:30 +0900 Subject: [PATCH 10/10] [debian] Implement ROS2 packaging --- .github/workflows/package.yml | 3 +- debian/control | 9 ++-- debian/control.ros | 12 +---- .../ros-ROS_DISTRO-mc-rtc-rviz-panel.install | 9 +++- debian/ros-ROS_DISTRO-mc-rtc-ticker.install | 5 +- debian/rules | 52 ++++++++++--------- 6 files changed, 49 insertions(+), 41 deletions(-) diff --git a/.github/workflows/package.yml b/.github/workflows/package.yml index cc85ff3..1cccc78 100644 --- a/.github/workflows/package.yml +++ b/.github/workflows/package.yml @@ -24,9 +24,10 @@ jobs: uses: jrl-umi3218/github-actions/.github/workflows/package-project.yml@master with: with-ros: true + latest-cmake: true matrix: | { - "dist": ["bionic", "focal"], + "dist": ["bionic", "focal", "jammy"], "arch": ["amd64"] } secrets: diff --git a/debian/control b/debian/control index f57b5f4..5dce8c4 100644 --- a/debian/control +++ b/debian/control @@ -8,9 +8,12 @@ Vcs-git: https://github.com/jrl-umi3218/mc_rtc_msgs Vcs-Browser: https://github.com/jrl-umi3218/mc_rtc_msgs Build-Depends: debhelper (>= 9), cmake, - qt5-default, + qtbase5-dev | qt5-default, libqwt-qt5-dev, # ros-@ROS_DISTRO@-mc-rtc-plugin, -# ros-@ROS_DISTRO@-rviz, -# ros-@ROS_DISTRO@-tf, +#ROS1 ros-@ROS_DISTRO@-rviz, +#ROS1 ros-@ROS_DISTRO@-tf, +#ROS2 ros-@ROS_DISTRO@-rviz2, +#ROS2 ros-@ROS_DISTRO@-tf2, +#ROS2 ros-@ROS_DISTRO@-visualization-msgs, libmc-rtc-dev diff --git a/debian/control.ros b/debian/control.ros index 7d93d7b..83e56f7 100644 --- a/debian/control.ros +++ b/debian/control.ros @@ -4,7 +4,8 @@ Architecture: any Section: science Depends: ${mic:Depends}, ${shlibs:Depends}, - ros-@ROS_DISTRO@-rviz +#ROS1 ros-@ROS_DISTRO@-rviz +#ROS2 ros-@ROS_DISTRO@-rviz2 Description: mc-rtc - RViZ panel RViZ plugin implementing mc_rtc GUI @@ -16,15 +17,6 @@ Depends: ${misc:Depends}, Description: mc-rtc - ROS ticker mc_rtc interface implementation using ROS for kinematic simulations -Package: ros-@ROS_DISTRO@-mc-surfaces-visualization -Architecture: any -Section: science -Depends: ${misc:Depends}, - ${shlibs:Depends}, - ros-@ROS_DISTRO@-rviz -Description: mc-rtc - Surfaces visualization - RViZ based tools to visualize surfaces - Package: ros-@ROS_DISTRO@-mc-rtc-tools Architecture: any Section: science diff --git a/debian/ros-ROS_DISTRO-mc-rtc-rviz-panel.install b/debian/ros-ROS_DISTRO-mc-rtc-rviz-panel.install index c6c61f0..3aa21e4 100644 --- a/debian/ros-ROS_DISTRO-mc-rtc-rviz-panel.install +++ b/debian/ros-ROS_DISTRO-mc-rtc-rviz-panel.install @@ -1,4 +1,9 @@ opt/ros/@ROS_DISTRO@/lib/libmc_rtc_rviz_panel.so -opt/ros/@ROS_DISTRO@/lib/mc_rtc_rviz_panel/mc_rtc_gui -opt/ros/@ROS_DISTRO@/lib/pkgconfig/mc_rtc_rviz_panel.pc +#ROS1 opt/ros/@ROS_DISTRO@/lib/mc_rtc_rviz_panel/mc_rtc_gui +#ROS1 opt/ros/@ROS_DISTRO@/lib/pkgconfig/mc_rtc_rviz_panel.pc +#ROS2 opt/ros/@ROS_DISTRO@/bin/mc_rtc_gui +#ROS2 opt/ros/@ROS_DISTRO@/share/ament_index/resource_index/package_run_dependencies/mc_rtc_rviz_panel +#ROS2 opt/ros/@ROS_DISTRO@/share/ament_index/resource_index/packages/mc_rtc_rviz_panel +#ROS2 opt/ros/@ROS_DISTRO@/share/ament_index/resource_index/parent_prefix_path/mc_rtc_rviz_panel +#ROS2 opt/ros/@ROS_DISTRO@/share/ament_index/resource_index/rviz_common__pluginlib__plugin/mc_rtc_rviz_panel opt/ros/@ROS_DISTRO@/share/mc_rtc_rviz_panel/* diff --git a/debian/ros-ROS_DISTRO-mc-rtc-ticker.install b/debian/ros-ROS_DISTRO-mc-rtc-ticker.install index 0c40a3e..d91b0fe 100644 --- a/debian/ros-ROS_DISTRO-mc-rtc-ticker.install +++ b/debian/ros-ROS_DISTRO-mc-rtc-ticker.install @@ -1,2 +1,5 @@ -opt/ros/@ROS_DISTRO@/lib/pkgconfig/mc_rtc_ticker.pc +#ROS1 opt/ros/@ROS_DISTRO@/lib/pkgconfig/mc_rtc_ticker.pc +#ROS2 opt/ros/@ROS_DISTRO@/share/ament_index/resource_index/package_run_dependencies/mc_rtc_ticker +#ROS2 opt/ros/@ROS_DISTRO@/share/ament_index/resource_index/packages/mc_rtc_ticker +#ROS2 opt/ros/@ROS_DISTRO@/share/ament_index/resource_index/parent_prefix_path/mc_rtc_ticker opt/ros/@ROS_DISTRO@/share/mc_rtc_ticker/* diff --git a/debian/rules b/debian/rules index 1485e38..2ce5661 100755 --- a/debian/rules +++ b/debian/rules @@ -1,39 +1,43 @@ #!/usr/bin/make -f # -*- makefile -*- +SRC:=$(CURDIR) TMP:=$(CURDIR)/debian/tmp -export ROS_DISTRO=@ROS_DISTRO@ -ifeq (${ROS_DISTRO},) - export CMAKE_OPTIONS= - export EXTRA_STEPS= +export ROS_DISTRO:=@ROS_DISTRO@ +export ROS_VERSION:=$(shell . /opt/ros/${ROS_DISTRO}/setup.sh && echo $$ROS_VERSION) +export ROS_PYTHON_VERSION:=$(shell . /opt/ros/${ROS_DISTRO}/setup.sh && echo $$ROS_PYTHON_VERSION) +ifeq (${ROS_PYTHON_VERSION}, 3) + export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python3/dist-packages:${PYTHONPATH} + export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python$(shell python3 -c "import sys; print(f'{sys.version_info.major}.{sys.version_info.minor}');")/site-packages:${PYTHONPATH} + export PYTHON_EXECUTABLE:=/usr/bin/python3 else - export ROS_VERSION:=$(shell . /opt/ros/${ROS_DISTRO}/setup.sh && echo $$ROS_VERSION) - export ROS_PYTHON_VERSION:=$(shell . /opt/ros/${ROS_DISTRO}/setup.sh && echo $$ROS_PYTHON_VERSION) - ifeq (${ROS_PYTHON_VERSION}, 3) - export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python3/dist-packages:${PYTHONPATH} - export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python$(shell python3 -c "import sys; print(f'{sys.version_info.major}.{sys.version_info.minor}');")/site-packages:${PYTHONPATH} - export PYTHON_EXECUTABLE=/usr/bin/python3 - else - export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python2.7/dist-packages:${PYTHONPATH} - export PYTHON_EXECUTABLE=/usr/bin/python - endif - export PKG_CONFIG_PATH=/opt/ros/${ROS_DISTRO}/lib/pkgconfig:$PKG_CONFIG_PATH - export ROS_MASTER_URI=http://localhost:11311 - export ROS_PACKAGE_PATH=/opt/ros/${ROS_DISTRO}/share - export LD_LIBRARY_PATH:=/opt/ros/${ROS_DISTRO}/lib:$LD_LIBRARY_PATH - export CMAKE_PREFIX_PATH=/opt/ros/${ROS_DISTRO}:$CMAKE_PREFIX_PATH - export CMAKE_OPTIONS=-DCMAKE_INSTALL_PREFIX=/opt/ros/${ROS_DISTRO} -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE} + export PYTHONPATH:=/opt/ros/${ROS_DISTRO}/lib/python2.7/dist-packages:${PYTHONPATH} + export PYTHON_EXECUTABLE:=/usr/bin/python endif +export PKG_CONFIG_PATH:=/opt/ros/${ROS_DISTRO}/lib/pkgconfig:$PKG_CONFIG_PATH +export ROS_MASTER_URI:=http://localhost:11311 +export ROS_PACKAGE_PATH:=/opt/ros/${ROS_DISTRO}/share +export LD_LIBRARY_PATH:=/opt/ros/${ROS_DISTRO}/lib:$LD_LIBRARY_PATH +export CMAKE_PREFIX_PATH:=/opt/ros/${ROS_DISTRO}:$CMAKE_PREFIX_PATH +# Default CMake options for Debian packaging +export CMAKE_OPTIONS:=-DCMAKE_BUILD_TYPE=None -DCMAKE_EXPORT_NO_PACKAGE_REGISTRY=ON -DCMAKE_FIND_PACKAGE_NO_PACKAGE_REGISTRY=ON "-GUnix Makefiles" -DCMAKE_VERBOSE_MAKEFILE=ON -DCMAKE_AUTOGEN_VERBOSE=ON +export CMAKE_OPTIONS:=${CMAKE_OPTIONS} -DCMAKE_INSTALL_PREFIX=/opt/ros/${ROS_DISTRO} -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE} %: dh $@ override_dh_auto_configure: - cp $(CURDIR)/debian/CMakeLists.txt $(CURDIR)/ - dh_auto_configure -- ${CMAKE_OPTIONS} + cmake -S $(SRC)/mc_rtc_ticker -B $(SRC)/build/mc_rtc_ticker ${CMAKE_OPTIONS} + cmake -S $(SRC)/mc_rtc_rviz_panel -B $(SRC)/build/mc_rtc_rviz_panel ${CMAKE_OPTIONS} + +override_dh_auto_build: + cmake --build $(SRC)/build/mc_rtc_ticker + cmake --build $(SRC)/build/mc_rtc_rviz_panel override_dh_auto_install: - dh_auto_install --destdir=$(TMP) - ${EXTRA_STEPS} + cd $(SRC)/build/mc_rtc_ticker && make -j1 install DESTDIR=$(TMP) AM_UPDATE_INFO_DIR=no + cd $(SRC)/build/mc_rtc_rviz_panel && make -j1 install DESTDIR=$(TMP) AM_UPDATE_INFO_DIR=no + +override_dh_auto_test: