From 419db3b3b0245e9857e212ef86e441e60a4b10e4 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 3 Mar 2022 17:07:54 +0000 Subject: [PATCH] Compilation fixes for Jammy and bring back Rolling CI (#1095) * Use jammy dockers and clang-format-12 * Fix unused depend, and move to python3-lxml * add ompl to repos, fix versions and ogre * Remove ogre keys * Fix boolean node operator * Stop building dockers on branch and fix servo null pointer * update pre-commit to clang-format-12 and pre-commit fixes * clang-format workaround and more pre-commit fixes --- .docker/ci/Dockerfile | 4 ++-- .docker/release/Dockerfile | 2 +- .github/workflows/ci.yaml | 8 ++++---- .github/workflows/format.yaml | 4 ++-- .pre-commit-config.yaml | 2 +- moveit2.repos | 6 +++++- moveit_kinematics/package.xml | 2 +- .../test/unit_tests/src/unittest_planning_context.cpp | 7 ++++--- .../src/unittest_trajectory_generator_common.cpp | 2 +- .../pick_place/src/approach_and_translate_stage.cpp | 6 +++--- .../include/moveit_servo/parameter_descriptor_builder.hpp | 1 + moveit_ros/moveit_servo/test/servo_launch_test_common.hpp | 2 +- .../planning_scene_monitor/planning_scene_monitor.h | 4 ++-- moveit_ros/visualization/package.xml | 1 - moveit_ros/warehouse/warehouse/src/broadcast.cpp | 3 ++- moveit_ros/warehouse/warehouse/src/import_from_text.cpp | 2 ++ moveit_setup_assistant/package.xml | 1 - moveit_setup_assistant/src/collisions_updater.cpp | 2 ++ moveit_setup_assistant/src/setup_assistant_main.cpp | 2 ++ .../src/tools/compute_default_collisions.cpp | 4 ++-- moveit_setup_assistant/src/tools/moveit_config_data.cpp | 4 ++-- 21 files changed, 40 insertions(+), 29 deletions(-) diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index a9be862e96..484f2ab84f 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -2,7 +2,7 @@ # ROS base image augmented with all MoveIt dependencies to use for CI ARG ROS_DISTRO=rolling -FROM ros:${ROS_DISTRO}-ros-base-focal +FROM ros:${ROS_DISTRO}-ros-base MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de ENV TERM xterm @@ -25,7 +25,7 @@ RUN \ # Some basic requirements wget git sudo curl \ # Preferred build tools - clang clang-format-10 clang-tidy clang-tools \ + clang clang-format-12 clang-tidy clang-tools \ ccache && \ # # Fetch all dependencies from moveit2.repos diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index bc8e7b2796..16ae7de119 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -2,7 +2,7 @@ # Full debian-based install of MoveIt using apt-get ARG ROS_DISTRO=rolling -FROM ros:${ROS_DISTRO}-ros-base-focal +FROM ros:${ROS_DISTRO}-ros-base MAINTAINER Robert Haschke rhaschke@techfak.uni-bielefeld.de # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index f4da39a5d6..04671ccf1e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -16,12 +16,12 @@ jobs: fail-fast: false matrix: env: - - IMAGE: galactic-ci + - IMAGE: rolling-ci CCOV: true - ROS_DISTRO: galactic - - IMAGE: galactic-ci-testing + ROS_DISTRO: rolling + - IMAGE: rolling-ci-testing IKFAST_TEST: true - ROS_DISTRO: galactic + ROS_DISTRO: rolling CLANG_TIDY: pedantic env: CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy" diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index ddfa93f93a..542457d8ee 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -17,8 +17,8 @@ jobs: steps: - uses: actions/checkout@v2 - uses: actions/setup-python@v2 - - name: Install clang-format-10 - run: sudo apt-get install clang-format-10 + - name: Install clang-format-12 + run: sudo apt-get install clang-format-12 - uses: pre-commit/action@v2.0.3 id: precommit - name: Upload pre-commit changes diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 962dc42249..3dd06105d2 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -42,7 +42,7 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format-10 + entry: clang-format-12 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] diff --git a/moveit2.repos b/moveit2.repos index 71ee4bf3e0..c74e6089ed 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -2,7 +2,7 @@ repositories: geometric_shapes: type: git url: https://github.com/ros-planning/geometric_shapes - version: 2.1.2 + version: ros2 moveit_msgs: type: git url: https://github.com/ros-planning/moveit_msgs @@ -19,3 +19,7 @@ repositories: type: git url: https://github.com/ros-controls/ros2_control.git version: 2.1.0 + ompl: + type: git + url: https://github.com/ompl/ompl.git + version: main diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index dff7eb2fc3..1e4063900f 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -35,7 +35,7 @@ urdfdom - python-lxml + python3-lxml moveit_ros_planning moveit_resources_fanuc_description diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp index 2987dfabbd..7e32289394 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_planning_context.cpp @@ -124,7 +124,7 @@ class PlanningContextTest : public ::testing::Test planning_scene::PlanningScenePtr scene(new planning_scene::PlanningScene(robot_model_)); moveit::core::RobotState current_state(robot_model_); current_state.setToDefaultValues(); - current_state.setJointGroupPositions(planning_group_, { 0, 1.57, 1.57, 0, 0.2, 0 }); + current_state.setJointGroupPositions(planning_group_, std::vector{ 0, 1.57, 1.57, 0, 0.2, 0 }); scene->setCurrentState(current_state); planning_context_->setPlanningScene(scene); // TODO Check what happens if this is missing } @@ -148,8 +148,9 @@ class PlanningContextTest : public ::testing::Test // state state in joint space, used as initial positions, since IK does not // work at zero positions rstate.setJointGroupPositions(this->planning_group_, - { 4.430233957464225e-12, 0.007881892504574495, -1.8157263253868452, - 1.1801525390026025e-11, 1.8236082178909834, 8.591793942969161e-12 }); + std::vector{ 4.430233957464225e-12, 0.007881892504574495, -1.8157263253868452, + 1.1801525390026025e-11, 1.8236082178909834, + 8.591793942969161e-12 }); Eigen::Isometry3d start_pose(Eigen::Isometry3d::Identity()); start_pose.translation() = Eigen::Vector3d(0.3, 0, 0.65); rstate.setFromIK(this->robot_model_->getJointModelGroup(this->planning_group_), start_pose); diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp index 06eabc333a..385e802b1f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_common.cpp @@ -142,7 +142,7 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test req_.max_acceleration_scaling_factor = 1.0; moveit::core::RobotState rstate(robot_model_); rstate.setToDefaultValues(); - rstate.setJointGroupPositions(planning_group_, { 0, M_PI / 2, 0, M_PI / 2, 0, 0 }); + rstate.setJointGroupPositions(planning_group_, std::vector{ 0, M_PI / 2, 0, M_PI / 2, 0, 0 }); rstate.setVariableVelocities(std::vector(rstate.getVariableCount(), 0.0)); moveit::core::robotStateToRobotStateMsg(rstate, req_.start_state, false); moveit_msgs::msg::Constraints goal_constraint; diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index 7e32d25d42..d31bab144b 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -147,9 +147,9 @@ bool executeAttachObject(const ManipulationPlanSharedDataConstPtr& shared_plan_d ok = ps->processAttachedCollisionObjectMsg(msg); } motion_plan->planning_scene_monitor_->triggerSceneUpdateEvent( - (planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType)( - planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY + - planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE)); + (planning_scene_monitor::PlanningSceneMonitor:: + SceneUpdateType)(planning_scene_monitor::PlanningSceneMonitor::UPDATE_GEOMETRY + + planning_scene_monitor::PlanningSceneMonitor::UPDATE_STATE)); return ok; } diff --git a/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp b/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp index 341aa796d8..6977a0d3e2 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include namespace moveit_servo diff --git a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp index 3680e01d00..ad5073cd23 100644 --- a/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp +++ b/moveit_ros/moveit_servo/test/servo_launch_test_common.hpp @@ -113,7 +113,7 @@ class ServoFixture : public ::testing::Test // Otherwise the Servo is still running when another test starts... if (!client_servo_stop_) { - client_servo_stop_->async_send_request(std::make_shared()); + stop(); } executor_->cancel(); if (executor_thread_.joinable()) diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h index 46a837d9df..32e48a589f 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h @@ -648,7 +648,7 @@ class LockedPlanningSceneRO return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene(); } - operator const planning_scene::PlanningSceneConstPtr &() const + operator const planning_scene::PlanningSceneConstPtr&() const { return static_cast(planning_scene_monitor_.get())->getPlanningScene(); } @@ -729,7 +729,7 @@ class LockedPlanningSceneRW : public LockedPlanningSceneRO { } - operator const planning_scene::PlanningScenePtr &() + operator const planning_scene::PlanningScenePtr&() { return planning_scene_monitor_->getPlanningScene(); } diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 2de19a78d4..07a82d9784 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -25,7 +25,6 @@ class_loader eigen - libogre-dev libqt5-opengl-dev qtbase5-dev diff --git a/moveit_ros/warehouse/warehouse/src/broadcast.cpp b/moveit_ros/warehouse/warehouse/src/broadcast.cpp index 5a340ae438..eafc03c17a 100644 --- a/moveit_ros/warehouse/warehouse/src/broadcast.cpp +++ b/moveit_ros/warehouse/warehouse/src/broadcast.cpp @@ -68,6 +68,7 @@ int main(int argc, char** argv) // time to wait in between publishing messages double delay = 0.001; + // clang-format off boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("host", boost::program_options::value(), "Host for the " @@ -80,7 +81,7 @@ int main(int argc, char** argv) "state", boost::program_options::value(), "Name of the robot state to publish.")("delay", boost::program_options::value()->default_value(delay), "Time to wait in between publishing messages (s)"); - + // clang-format on boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); boost::program_options::notify(vm); diff --git a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp index c1d56be830..0ca7394679 100644 --- a/moveit_ros/warehouse/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/warehouse/src/import_from_text.cpp @@ -216,12 +216,14 @@ int main(int argc, char** argv) node_options.automatically_declare_parameters_from_overrides(true); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("import_from_text_to_warehouse", node_options); + // clang-format off boost::program_options::options_description desc; desc.add_options()("help", "Show help message")("queries", boost::program_options::value(), "Name of file containing motion planning queries.")( "scene", boost::program_options::value(), "Name of file containing motion planning scene.")( "host", boost::program_options::value(), "Host for the DB.")("port", boost::program_options::value(), "Port for the DB."); + // clang-format on boost::program_options::variables_map vm; boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm); diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 6f74c84ad9..fb1192ed95 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -20,7 +20,6 @@ ament_cmake moveit_common - libogre-dev ompl ament_index_cpp diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp index 5264d7872e..35533fa6d9 100644 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/src/collisions_updater.cpp @@ -127,6 +127,7 @@ int main(int argc, char* argv[]) uint32_t never_trials = 0; + // clang-format off po::options_description desc("Allowed options"); desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to MoveIt config package")( "urdf", po::value(&urdf_path), "path to URDF ( or xacro)")("srdf", po::value(&srdf_path), @@ -141,6 +142,7 @@ int main(int argc, char* argv[]) "number of trials for searching never colliding pairs")( "min-collision-fraction", po::value(&min_collision_fraction), "fraction of small sample size to determine links that are always colliding"); + // clang-format on po::positional_options_description pos_desc; pos_desc.add("xacro-args", -1); diff --git a/moveit_setup_assistant/src/setup_assistant_main.cpp b/moveit_setup_assistant/src/setup_assistant_main.cpp index 7865772c6c..2a22178710 100644 --- a/moveit_setup_assistant/src/setup_assistant_main.cpp +++ b/moveit_setup_assistant/src/setup_assistant_main.cpp @@ -59,10 +59,12 @@ int main(int argc, char** argv) namespace po = boost::program_options; // Declare the supported options + // clang-format off po::options_description desc("Allowed options"); desc.add_options()("help,h", "Show help message")("debug,g", "Run in debug/test mode")( "urdf_path,u", po::value(), "Optional, path to URDF file in ROS package")( "config_pkg,c", po::value(), "Optional, pass in existing config package to load"); + // clang-format on // Process options po::variables_map vm; diff --git a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp index e21eb7eeb1..b6f2cd8fed 100644 --- a/moveit_setup_assistant/src/tools/compute_default_collisions.cpp +++ b/moveit_setup_assistant/src/tools/compute_default_collisions.cpp @@ -52,8 +52,8 @@ const boost::unordered_map REASONS_TO_STRING = boos DEFAULT, "Default")(ADJACENT, "Adjacent")(ALWAYS, "Always")(USER, "User")(NOT_DISABLED, "Not Disabled"); const boost::unordered_map REASONS_FROM_STRING = - boost::assign::map_list_of("Never", NEVER)("Default", DEFAULT)("Adjacent", ADJACENT)("Always", ALWAYS)( - "User", USER)("Not Disabled", NOT_DISABLED); + boost::assign::map_list_of("Never", NEVER)("Default", DEFAULT)( + "Adjacent", ADJACENT)("Always", ALWAYS)("User", USER)("Not Disabled", NOT_DISABLED); // Used for logging static const rclcpp::Logger LOGGER = rclcpp::get_logger("collision_updater"); diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 491b5a6d7b..5542eebfeb 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -1207,7 +1207,7 @@ class SortableDisabledCollision : dc_(dc), key_(dc.link1_ < dc.link2_ ? (dc.link1_ + "|" + dc.link2_) : (dc.link2_ + "|" + dc.link1_)) { } - operator const srdf::Model::DisabledCollision &() const + operator const srdf::Model::DisabledCollision&() const { return dc_; } @@ -1287,7 +1287,7 @@ template bool parse(const YAML::Node& node, const std::string& key, T& storage, const T& default_value = T()) { const YAML::Node& n = node[key]; - bool valid = n; + bool valid = n.IsDefined(); storage = valid ? n.as() : default_value; return valid; }