From 30512bcbb4ecec26ee05614daaf2e5815d521af8 Mon Sep 17 00:00:00 2001 From: Niklas Fiedler Date: Fri, 22 Jul 2022 13:38:05 +0200 Subject: [PATCH 01/23] Report 1st collision pair for invalid IK solutions (#376) --- core/src/stages/compute_ik.cpp | 42 +++++++++++++++++++++++++--------- 1 file changed, 31 insertions(+), 11 deletions(-) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 77aa8bd3..e5409b9a 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -84,7 +84,15 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& } // found IK solutions -using IKSolutions = std::vector>; + +struct IKSolution +{ + std::vector joint_positions; + bool feasible; + collision_detection::Contact contact; +}; + +using IKSolutions = std::vector; namespace { @@ -353,14 +361,23 @@ void ComputeIK::compute() { &ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const double* joint_positions) { for (const auto& sol : ik_solutions) { - if (jmg->distance(joint_positions, sol.data()) < min_solution_distance) + if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance) return false; // too close to already found solution } state->setJointGroupPositions(jmg, joint_positions); ik_solutions.emplace_back(); - state->copyJointGroupPositions(jmg, ik_solutions.back()); - - return ignore_collisions || !scene->isStateColliding(*state, jmg->getName()); + auto& solution{ ik_solutions.back() }; + state->copyJointGroupPositions(jmg, solution.joint_positions); + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.contacts = true; + req.max_contacts = 1; + scene->checkCollision(req, res, *state); + solution.feasible = ignore_collisions || !res.collision; + if (res.contacts.size() > 0) { + solution.contact = res.contacts.begin()->second.front(); + } + return solution.feasible; }; uint32_t max_ik_solutions = props.get("max_ik_solutions"); @@ -388,15 +405,18 @@ void ComputeIK::compute() { solution.setComment(s.comment()); std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers())); - if (succeeded && i + 1 == ik_solutions.size()) + if (ik_solutions[i].feasible) // compute cost as distance to compare_pose - solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); - else // found an IK solution, but this was not valid - solution.markAsFailure(); - + solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data())); + else { // found an IK solution, but this was not valid + std::stringstream ss; + ss << "Collision between '" << ik_solutions[i].contact.body_name_1 << "' and '" + << ik_solutions[i].contact.body_name_2 << "'"; + solution.markAsFailure(ss.str()); + } // set scene's robot state robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); - solution_state.setJointGroupPositions(jmg, ik_solutions[i].data()); + solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data()); solution_state.update(); InterfaceState state(solution_scene); From 6df359ed3b3b83998310155452476b6b53650366 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 5 Oct 2022 12:47:03 +0200 Subject: [PATCH 02/23] Do not dictate C++ standard C++14 is default in clang/gcc anyway and latest log4cxx requires C++17. Qt on Ubuntu 18.04 sets C++11. Hence we use MoveIt's cmake macro to ensure C++14 at least. --- capabilities/CMakeLists.txt | 4 ++-- core/CMakeLists.txt | 4 ++-- demo/CMakeLists.txt | 8 ++------ rviz_marker_tools/CMakeLists.txt | 2 -- visualization/CMakeLists.txt | 4 ++-- 5 files changed, 8 insertions(+), 14 deletions(-) diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index 539ba41f..e5e1a5c4 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_capabilities) -add_compile_options(-std=c++14) - find_package(catkin REQUIRED COMPONENTS actionlib moveit_core @@ -13,6 +11,8 @@ find_package(catkin REQUIRED COMPONENTS std_msgs ) +moveit_build_options() + catkin_package( LIBRARIES $PROJECT_NAME} CATKIN_DEPENDS diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 5a0c704f..5703dbd1 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -14,6 +14,8 @@ find_package(catkin REQUIRED COMPONENTS rviz_marker_tools ) +moveit_build_options() + catkin_package( LIBRARIES ${PROJECT_NAME} @@ -29,8 +31,6 @@ catkin_package( visualization_msgs ) -set(CMAKE_CXX_STANDARD 14) - add_compile_options(-fvisibility-inlines-hidden) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 32b287f1..1763140a 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -1,12 +1,6 @@ cmake_minimum_required(VERSION 3.1.3) project(moveit_task_constructor_demo) -if(NOT "${CMAKE_CXX_STANDARD}") - set(CMAKE_CXX_STANDARD 14) -endif() -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - find_package(catkin REQUIRED COMPONENTS roscpp moveit_core @@ -15,6 +9,8 @@ find_package(catkin REQUIRED COMPONENTS rosparam_shortcuts ) +moveit_build_options() + catkin_package( CATKIN_DEPENDS roscpp ) diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index d7afda22..885b05e6 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -20,8 +20,6 @@ catkin_package( rviz ) -set(CMAKE_CXX_STANDARD 14) - set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}) set(HEADERS diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 5065bc9f..9185ab3b 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -10,6 +10,8 @@ find_package(catkin REQUIRED COMPONENTS rviz ) +moveit_build_options() + # rviz transitively includes OGRE headers which break with `-Wall -Werror` # so isolate these include dirs and add them as SYSTEM include where needed. set(rviz_OGRE_INCLUDE_DIRS) @@ -57,8 +59,6 @@ catkin_package( rviz ) -set(CMAKE_CXX_STANDARD 14) - add_subdirectory(visualization_tools) add_subdirectory(motion_planning_tasks) From f6b925c2fe0224e451b7036c4bfe61da0c6962d0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 23 Oct 2022 11:54:51 +0200 Subject: [PATCH 03/23] Drop support for MoveIt's melodic-devel branch --- .github/workflows/ci.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 2d46fd75..53cd469a 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -14,13 +14,13 @@ jobs: fail-fast: false matrix: env: - - IMAGE: melodic-source - NAME: ccov - TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: master-source CXXFLAGS: >- -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter + - IMAGE: noetic-source + NAME: ccov + TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: noetic-source CXX: clang++ CLANG_TIDY: true From 28fb974adbfaac8268489df94625f8702e3705ff Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 23 Oct 2022 13:37:23 +0200 Subject: [PATCH 04/23] Remove MoveIt compatibility code --- .../src/execute_task_solution_capability.cpp | 12 ----------- .../moveit/task_constructor/moveit_compat.h | 21 ------------------- core/include/moveit/task_constructor/utils.h | 3 --- core/src/container.cpp | 1 - core/src/cost_terms.cpp | 10 --------- core/src/solvers/cartesian_path.cpp | 10 --------- core/src/solvers/joint_interpolation.cpp | 1 - core/src/solvers/planner_interface.cpp | 1 - core/src/stage.cpp | 9 -------- core/src/stages/compute_ik.cpp | 3 +-- core/src/stages/connect.cpp | 1 - core/src/stages/fix_collision_objects.cpp | 6 ------ core/src/stages/move_to.cpp | 1 - core/src/utils.cpp | 19 +---------------- core/test/test_move_relative.cpp | 6 ------ core/test/test_move_to.cpp | 13 ------------ demo/src/fallbacks_move_to.cpp | 7 ------- 17 files changed, 2 insertions(+), 122 deletions(-) diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 482f5260..51bfc3d2 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -36,16 +36,12 @@ #include "execute_task_solution_capability.h" -#include - #include #include #include #include #include -#if MOVEIT_HAS_MESSAGE_CHECKS #include -#endif namespace { @@ -170,22 +166,14 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr /* TODO add action feedback and markers */ exec_traj.effect_on_success_ = [this, sub_traj, description](const plan_execution::ExecutableMotionPlan* /*plan*/) { -#if MOVEIT_HAS_MESSAGE_CHECKS if (!moveit::core::isEmpty(sub_traj.scene_diff)) { -#else - if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) { -#endif ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description); return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff); } return true; }; -#if MOVEIT_HAS_MESSAGE_CHECKS if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) && -#else - if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) && -#endif !moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) { ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "invalid intermediate robot state in scene diff of SubTrajectory " << description); diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index 202a47a9..f4893507 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -44,24 +44,3 @@ #define MOVEIT_VERSION_GE(major, minor, patch) \ (MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \ major * 1'000'000 + minor * 1'000 + patch) - -// use collision env instead of collision robot/world -#define MOVEIT_HAS_COLLISION_ENV MOVEIT_VERSION_GE(1, 1, 0) - -// cartesian interpolator got separated from RobotState at some point -#define MOVEIT_HAS_CARTESIAN_INTERPOLATOR MOVEIT_VERSION_GE(1, 1, 0) - -// isEmpty got split off into its own header -#define MOVEIT_HAS_MESSAGE_CHECKS MOVEIT_VERSION_GE(1, 1, 0) - -// use object shape poses relative to a single object pose -#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6) - -#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6) - -#if !MOVEIT_VERSION_GE(1, 1, 9) -// the pointers are not yet available -namespace trajectory_processing { -MOVEIT_CLASS_FORWARD(TimeParameterization); -} -#endif diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 70b89482..fcc6f591 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -140,9 +140,6 @@ private: Int i; }; -const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, - std::string frame); - bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, const moveit::core::JointModelGroup* jmg, SolutionBase& solution, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame); diff --git a/core/src/container.cpp b/core/src/container.cpp index f9b79cfe..3c9ae228 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 130a9bb3..0c313bec 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -36,7 +36,6 @@ #include #include -#include #include #include @@ -196,18 +195,9 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { collision_detection::DistanceResult result; if (with_world) -#if MOVEIT_HAS_COLLISION_ENV state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); -#else - state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(), - robot); -#endif else -#if MOVEIT_HAS_COLLISION_ENV state->scene()->getCollisionEnv()->distanceSelf(request, result, robot); -#else - state->scene()->getCollisionRobot()->distanceSelf(request, result, robot); -#endif if (result.minimum_distance.distance <= 0) { return result.minimum_distance; diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 3caecd75..a16b234b 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -37,14 +37,10 @@ */ #include -#include - #include #include #include -#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR #include -#endif using namespace trajectory_processing; @@ -96,17 +92,11 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons }; std::vector trajectory; -#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, moveit::core::MaxEEFStep(props.get("step_size")), moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, props.get("kinematics_options")); -#else - double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath( - jmg, trajectory, &link, target, true, props.get("step_size"), props.get("jump_threshold"), - is_valid, props.get("kinematics_options")); -#endif assert(!trajectory.empty()); // there should be at least the start state result = std::make_shared(sandbox_scene->getRobotModel(), jmg); diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index 61e640c2..fc687613 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -37,7 +37,6 @@ */ #include -#include #include #include diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp index bb2a49e7..617fb476 100644 --- a/core/src/solvers/planner_interface.cpp +++ b/core/src/solvers/planner_interface.cpp @@ -37,7 +37,6 @@ */ #include -#include #include using namespace trajectory_processing; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 5ad85e49..60315e57 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -38,7 +38,6 @@ #include #include #include -#include #include @@ -894,12 +893,10 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta return false; } -#if MOVEIT_HAS_OBJECT_POSE if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) { ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name); return false; // transforms do not match } -#endif if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) { ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name); @@ -946,15 +943,9 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta return false; // shapes not matching } -#if MOVEIT_HAS_OBJECT_POSE auto from_it = from_object->getShapePosesInLinkFrame().cbegin(); auto from_end = from_object->getShapePosesInLinkFrame().cend(); auto to_it = to_object->getShapePosesInLinkFrame().cbegin(); -#else - auto from_it = from_object->getFixedTransforms().cbegin(); - auto from_end = from_object->getFixedTransforms().cend(); - auto to_it = to_object->getFixedTransforms().cbegin(); -#endif for (; from_it != from_end; ++from_it, ++to_it) if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) { ROS_DEBUG_STREAM_NAMED("Connecting", diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index e5409b9a..2b8ece2a 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include @@ -302,7 +301,7 @@ void ComputeIK::compute() { } ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; - link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id); + link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id); // transform target pose such that ik frame will reach there if link does target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName()); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index ba6483e4..16e2ea8f 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -38,7 +38,6 @@ #include #include -#include #include #include diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index cf382ea2..cea34e0a 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -38,7 +38,6 @@ #include #include -#include #include #include @@ -117,13 +116,8 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene& bool failure = false; while (!failure) { res.clear(); -#if MOVEIT_HAS_COLLISION_ENV scene.getCollisionEnv()->checkRobotCollision(req, res, scene.getCurrentState(), scene.getAllowedCollisionMatrix()); -#else - scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), - scene.getCurrentState(), scene.getAllowedCollisionMatrix()); -#endif if (!res.collision) return result; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 3270a831..5611475c 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -42,7 +42,6 @@ #include #include -#include #include #include diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 0a4e7e0e..8b7c484b 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -40,7 +40,6 @@ #include #include -#include #include #include @@ -48,22 +47,6 @@ namespace moveit { namespace task_constructor { namespace utils { -const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, - std::string frame) { -#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK - return state.getRigidlyConnectedParentLinkModel(frame); -#else - const moveit::core::LinkModel* link{ nullptr }; - - if (state.hasAttachedBody(frame)) { - link = state.getAttachedBody(frame)->getAttachedLink(); - } else if (state.getRobotModel()->hasLinkModel(frame)) - link = state.getLinkModel(frame); - - return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link); -#endif -} - bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, const moveit::core::JointModelGroup* jmg, SolutionBase& solution, const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) { @@ -94,7 +77,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame; } else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) { - robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id); + robot_link = scene.getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id); tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; } else { diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 8b59074a..a7f2d4d3 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -4,7 +4,6 @@ #include #include #include -#include #include @@ -63,12 +62,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) geometry_msgs::Pose p; p.position.x = 0.1; p.orientation.w = 1.0; -#if MOVEIT_HAS_OBJECT_POSE aco.object.pose = p; -#else - aco.object.primitive_poses.resize(1, p); - aco.object.primitive_poses[0] = p; -#endif return aco; } diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 19fdaf07..83d6b076 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include @@ -118,23 +117,14 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) p.dimensions[p.SPHERE_RADIUS] = 0.01; return p; }()); -#if MOVEIT_HAS_OBJECT_POSE aco.object.pose.position.z = 0.2; aco.object.pose.orientation.w = 1.0; -#else - aco.object.primitive_poses.resize(1); - aco.object.primitive_poses[0].position.z = 0.2; - aco.object.primitive_poses[0].orientation.w = 1.0; -#endif -#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK - // If we don't have this, we also don't have subframe support aco.object.subframe_names.resize(1, "subframe"); aco.object.subframe_poses.resize(1, [] { geometry_msgs::Pose p; p.orientation.w = 1.0; return p; }()); -#endif return aco; } @@ -147,8 +137,6 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) { EXPECT_ONE_SOLUTION; } -#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK -// If we don't have this, we also don't have subframe support TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { const std::string ATTACHED_OBJECT{ "attached_object" }; const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; @@ -159,7 +147,6 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); EXPECT_ONE_SOLUTION; } -#endif // This test require a running rosmaster TEST(Task, taskMoveConstructor) { diff --git a/demo/src/fallbacks_move_to.cpp b/demo/src/fallbacks_move_to.cpp index 0a586feb..a0f2d4ea 100644 --- a/demo/src/fallbacks_move_to.cpp +++ b/demo/src/fallbacks_move_to.cpp @@ -3,8 +3,6 @@ #include #include -#include - #include #include #include @@ -85,12 +83,7 @@ int main(int argc, char** argv) { co.id = "box"; co.header.frame_id = "panda_link0"; co.operation = co.ADD; -#if MOVEIT_HAS_OBJECT_POSE auto& pose{ co.pose }; -#else - co.primitive_poses.emplace_back(); - auto& pose{ co.primitive_poses[0] }; -#endif pose = []() { geometry_msgs::Pose p; p.position.x = 0.3; From 987c1485a6e72e6b9037239ca0c3df6fee076954 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 23 Oct 2022 13:49:54 +0200 Subject: [PATCH 05/23] Implicitly use clang in clang-tidy builds --- .github/workflows/ci.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 53cd469a..ea9207a0 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -22,7 +22,6 @@ jobs: NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: noetic-source - CXX: clang++ CLANG_TIDY: true CXXFLAGS: >- -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls From b5f5c1fef87ba43a6d78183358c963a314da71a7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 23 Oct 2022 14:00:45 +0200 Subject: [PATCH 06/23] Limit strict compiler warnings to target workspace --- .github/workflows/ci.yaml | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index ea9207a0..e5bac391 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -15,23 +15,25 @@ jobs: matrix: env: - IMAGE: master-source - CXXFLAGS: >- - -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls - -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter + TARGET_CMAKE_ARGS: >- + -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=" + -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls + -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter" - IMAGE: noetic-source NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: noetic-source CLANG_TIDY: true - CXXFLAGS: >- - -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls - -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy + TARGET_CMAKE_ARGS: >- + -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=" + -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls + -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy" - IMAGE: noetic-source NAME: asan DOCKER_RUN_OPTS: >- -e PRELOAD=libasan.so.5 -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions" - TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g" + TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g" env: CATKIN_LINT: true From c0cd6dcd01bd1a42e43a2a57ae9ec4f66e0fd134 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 23 Oct 2022 16:53:26 +0200 Subject: [PATCH 07/23] Update GHA versions --- .github/workflows/ci.yaml | 10 ++++++---- .github/workflows/format.yaml | 5 ++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e5bac391..1c303e77 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -52,16 +52,18 @@ jobs: name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}" runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: Cache ccache - uses: pat-s/always-upload-cache@v2.1.5 + uses: rhaschke/cache@main with: path: ${{ env.CCACHE_DIR }} key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} + env: + GHA_CACHE_SAVE: always - id: ici name: Run industrial_ci @@ -69,7 +71,7 @@ jobs: env: ${{ matrix.env }} - name: Upload test artifacts (on failure) - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3 if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) with: name: test-results-${{ matrix.env.IMAGE }} @@ -82,7 +84,7 @@ jobs: workdir: ${{ env.BASEDIR }}/target_ws ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' - name: Upload codecov report - uses: codecov/codecov-action@v2 + uses: codecov/codecov-action@v3 if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/target_ws/coverage.info diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 05f532be..9d09e452 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -13,14 +13,13 @@ jobs: name: pre-commit runs-on: ubuntu-latest steps: - - uses: actions/checkout@v2 - - uses: actions/setup-python@v2 + - uses: actions/checkout@v3 - name: Install clang-format-10 run: sudo apt-get install clang-format-10 - uses: rhaschke/install-catkin_lint-action@v1.0 with: distro: noetic - - uses: pre-commit/action@v2.0.0 + - uses: pre-commit/action@v3.0.0 id: precommit - name: Upload pre-commit changes if: failure() && steps.precommit.outcome == 'failure' From 2b2689e8d788f1e52c100d6cd5720850911a50cb Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 24 Oct 2022 14:15:50 +0900 Subject: [PATCH 08/23] Use catkin_INCLUDE_DIRS as system includes ... to suppress warnings outside the code base --- capabilities/CMakeLists.txt | 5 +---- core/src/CMakeLists.txt | 2 +- demo/CMakeLists.txt | 10 ++++------ rviz_marker_tools/CMakeLists.txt | 6 ++---- .../properties/CMakeLists.txt | 10 +++------- .../motion_planning_tasks/src/CMakeLists.txt | 14 +++++++------- .../motion_planning_tasks/utils/CMakeLists.txt | 10 +++------- visualization/visualization_tools/CMakeLists.txt | 8 +++----- 8 files changed, 24 insertions(+), 41 deletions(-) diff --git a/capabilities/CMakeLists.txt b/capabilities/CMakeLists.txt index e5e1a5c4..cff07090 100644 --- a/capabilities/CMakeLists.txt +++ b/capabilities/CMakeLists.txt @@ -21,14 +21,11 @@ catkin_package( std_msgs ) -include_directories( - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME} src/execute_task_solution_capability.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) install(TARGETS ${PROJECT_NAME} diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index d37ea27c..e25a036c 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -39,8 +39,8 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} PUBLIC $ PUBLIC $ - PUBLIC ${catkin_INCLUDE_DIRS} ) +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) add_subdirectory(stages) diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 1763140a..a421eefc 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -15,13 +15,10 @@ catkin_package( CATKIN_DEPENDS roscpp ) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - add_library(${PROJECT_NAME}_pick_place_task src/pick_place_task.cpp) target_link_libraries(${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES}) +target_include_directories(${PROJECT_NAME}_pick_place_task PUBLIC include) +target_include_directories(${PROJECT_NAME}_pick_place_task SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) add_dependencies(${PROJECT_NAME}_pick_place_task ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) install(TARGETS ${PROJECT_NAME}_pick_place_task ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -34,6 +31,7 @@ function(demo name) add_executable(${PROJECT_NAME}_${name} src/${name}.cpp) add_dependencies(${PROJECT_NAME}_${name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME}_${name} ${catkin_LIBRARIES}) + target_include_directories(${PROJECT_NAME}_${name} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS}) set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "") install(TARGETS ${PROJECT_NAME}_${name} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -52,7 +50,7 @@ demo(pick_place_demo) target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task) install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) add_subdirectory(test) diff --git a/rviz_marker_tools/CMakeLists.txt b/rviz_marker_tools/CMakeLists.txt index 885b05e6..7312cb71 100644 --- a/rviz_marker_tools/CMakeLists.txt +++ b/rviz_marker_tools/CMakeLists.txt @@ -32,10 +32,8 @@ add_library(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -target_include_directories(${PROJECT_NAME} - PUBLIC include - PRIVATE ${catkin_INCLUDE_DIRS} -) +target_include_directories(${PROJECT_NAME} PUBLIC $) +target_include_directories(${PROJECT_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS}) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index b0fabdaf..f9fa7de0 100644 --- a/visualization/motion_planning_tasks/properties/CMakeLists.txt +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -15,13 +15,9 @@ endif() add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) -target_link_libraries(${MOVEIT_LIB_NAME} - ${QT_LIBRARIES} ${YAML_LIBRARIES} -) -target_include_directories(${MOVEIT_LIB_NAME} - PUBLIC $ - PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS} -) +target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES} ${YAML_LIBRARIES}) +target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $) +target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index 832134c7..aba4de28 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -33,14 +33,14 @@ target_link_libraries(${MOVEIT_LIB_NAME} target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ PRIVATE $ - # https://stackoverflow.com/questions/47175683/cmake-target-link-libraries-propagation-of-include-directories - PUBLIC $ - PUBLIC $ - PUBLIC $ - PUBLIC ${catkin_INCLUDE_DIRS} ) -target_include_directories(${MOVEIT_LIB_NAME} SYSTEM - PUBLIC ${rviz_OGRE_INCLUDE_DIRS} +target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PUBLIC + # https://stackoverflow.com/questions/47175683/cmake-target-link-libraries-propagation-of-include-directories + $ + $ + $ + ${catkin_INCLUDE_DIRS} + ${rviz_OGRE_INCLUDE_DIRS} ) install(TARGETS ${MOVEIT_LIB_NAME} diff --git a/visualization/motion_planning_tasks/utils/CMakeLists.txt b/visualization/motion_planning_tasks/utils/CMakeLists.txt index cab82cae..1e07ad8e 100644 --- a/visualization/motion_planning_tasks/utils/CMakeLists.txt +++ b/visualization/motion_planning_tasks/utils/CMakeLists.txt @@ -7,13 +7,9 @@ set(SOURCES ) add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) -target_link_libraries(${MOVEIT_LIB_NAME} - ${QT_LIBRARIES} -) -target_include_directories(${MOVEIT_LIB_NAME} - PUBLIC $ - PRIVATE ${catkin_INCLUDE_DIRS} -) +target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES}) +target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $) +target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS}) install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 49116a1e..de0eb39d 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -31,12 +31,10 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES} ${Boost_LIBRARIES} ) -target_include_directories(${MOVEIT_LIB_NAME} - PUBLIC include - PRIVATE ${catkin_INCLUDE_DIRS} -) +target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include) target_include_directories(${MOVEIT_LIB_NAME} SYSTEM - PUBLIC ${rviz_OGRE_INCLUDE_DIRS} + PRIVATE ${catkin_INCLUDE_DIRS} + PUBLIC ${rviz_OGRE_INCLUDE_DIRS} ) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) From 5a30f8a8a716a239ffce468e0834d89619637bf3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 24 Oct 2022 10:03:48 +0200 Subject: [PATCH 09/23] Suppress unused-function warning --- core/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 5703dbd1..445569f6 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS rviz_marker_tools ) +option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings moveit_build_options() catkin_package( From c60cd6c86fac953bbd4ab0c73a1fffc00ad3969a Mon Sep 17 00:00:00 2001 From: v4hn Date: Sun, 23 Oct 2022 23:53:21 +0900 Subject: [PATCH 10/23] Avoid unused-parameter warnings --- core/src/container.cpp | 2 +- core/src/solvers/cartesian_path.cpp | 2 +- .../motion_planning_tasks/properties/property_factory.cpp | 2 +- visualization/motion_planning_tasks/utils/icon.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index f9b79cfe..f118b652 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -1047,7 +1047,7 @@ void FallbacksPrivateConnect::compute() { void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) { // expect failure to be reported from active child - assert(active_ != children().end() && active_->get() == &child); + assert(active_ != children().end() && active_->get() == &child); (void)child; // ... thus we can use std::next(active_) to find the next child auto next = std::next(active_); diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 3caecd75..c22cc09b 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -78,7 +78,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, } bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, + const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 95542a74..bc347391 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -148,7 +148,7 @@ void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::Property } #ifndef HAVE_YAML -rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, +rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, const std::string& description, const std::string& value, rviz::Property* old) { if (old) { // reuse existing Property? diff --git a/visualization/motion_planning_tasks/utils/icon.cpp b/visualization/motion_planning_tasks/utils/icon.cpp index 2296d720..0ee0db95 100644 --- a/visualization/motion_planning_tasks/utils/icon.cpp +++ b/visualization/motion_planning_tasks/utils/icon.cpp @@ -55,7 +55,7 @@ static QPixmap maskToColorAndAlpha(const QPixmap& mask, const QColor& color) { using MaskAndColor = QPair; using MasksAndColors = QList; -static MasksAndColors masksAndColors(const Icon& icon, int dpr) { +static MasksAndColors masksAndColors(const Icon& icon, int /*dpr*/) { MasksAndColors result; for (const IconMaskAndColor& i : icon) { const QString& file_name = i.first; From 4c9b16511b99080e05fc1182376483856956d615 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 23 Oct 2022 18:41:15 +0200 Subject: [PATCH 11/23] Hopefully fix spurious test failure --- core/src/task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/task.cpp b/core/src/task.cpp index a3535b4c..24320526 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -247,7 +247,7 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) { while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) { if (impl->preempt_requested_) return success_or(moveit::core::MoveItErrorCode::PREEMPTED); - if (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() > available_time) + if (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() >= available_time) return success_or(moveit::core::MoveItErrorCode::TIMED_OUT); compute(); for (const auto& cb : impl->task_cbs_) From b69366456d11b524a448102fca38e1c65b61016c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 23 Oct 2022 22:09:55 +0200 Subject: [PATCH 12/23] Fix TaskListModelTest.deletion: run required Qt event loop --- .../motion_planning_tasks/test/test_task_model.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 86909c20..8060235c 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -44,6 +44,7 @@ #include #include #include +#include using namespace moveit::task_constructor; @@ -226,12 +227,17 @@ TEST_F(TaskListModelTest, deletion) { // process deleteLater() events QCoreApplication::sendPostedEvents(nullptr, QEvent::DeferredDelete); // as m is owned by model, m should be destroyed - // EXPECT_EQ(num_deletes, 1); // TODO: event is not processed, missing event loop? + EXPECT_EQ(num_deletes, 1); EXPECT_EQ(model.rowCount(), 0); } int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); ros::init(argc, argv, "test_task_model"); - return RUN_ALL_TESTS(); + QCoreApplication app(argc, argv); + QTimer::singleShot(0, [&]() { + ::testing::InitGoogleTest(&argc, argv); + auto testResult = RUN_ALL_TESTS(); + app.exit(testResult); + }); + return app.exec(); } From 402d6a4bfedee9b12fc1397dc21b78e2d869b55c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 31 Oct 2022 16:10:59 +0100 Subject: [PATCH 13/23] Improve unittest for move_relative --- core/test/test_move_relative.cpp | 53 ++++++++++++++++++++------------ 1 file changed, 34 insertions(+), 19 deletions(-) diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index a7f2d4d3..37c12705 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -17,7 +18,7 @@ using namespace planning_scene; using namespace moveit::core; constexpr double TAU{ 2 * M_PI }; -constexpr double EPS{ 1e-6 }; +constexpr double EPS{ 5e-5 }; // provide a basic test fixture that prepares a Task struct PandaMoveRelative : public testing::Test @@ -66,10 +67,24 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) return aco; } -inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) { - return scene->getFrameTransform(frame).translation(); +void expect_const_position(const SolutionBaseConstPtr& solution, const std::string& tip, + const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) { + const robot_trajectory::RobotTrajectory& t = *std::dynamic_pointer_cast(solution)->trajectory(); + const Eigen::Vector3d start_position = (t.getFirstWayPoint().getFrameTransform(tip) * offset).translation(); + for (size_t i = 0; i < t.getWayPointCount(); ++i) { + const Eigen::Vector3d position = (t.getWayPoint(i).getFrameTransform(tip) * offset).translation(); + ASSERT_TRUE(start_position.isApprox(position, EPS)) + << "Rotation must maintain position\n" + << i << ": " << start_position.transpose() << " != " << position.transpose(); + } } +#define EXPECT_CONST_POSITION(...) \ + { \ + SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \ + expect_const_position(__VA_ARGS__); \ + } + TEST_F(PandaMoveRelative, cartesianRotateEEF) { move->setDirection([] { geometry_msgs::TwistStamped twist; @@ -79,15 +94,22 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) { }()); ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName()); +} - const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() }; - const auto start_eef_position{ position(scene, tip_name) }; - const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) }; +TEST_F(PandaMoveRelative, cartesianCircular) { + const std::string tip = "panda_hand"; + auto offset = Eigen::Translation3d(0, 0, 0.1); + move->setIKFrame(offset, tip); + move->setDirection([] { + geometry_msgs::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.x = TAU / 4.0; + return twist; + }()); - EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) - << "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n" - << start_eef_position << "\nvs\n" - << end_eef_position; + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset)); } TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { @@ -102,15 +124,8 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { return twist; }()); - ASSERT_TRUE(t.plan()); - - const auto start_eef_position{ position(scene, ATTACHED_OBJECT) }; - const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) }; - - EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) - << "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n" - << start_eef_position << "\nvs\n" - << end_eef_position; + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), ATTACHED_OBJECT); } int main(int argc, char** argv) { From 076957d4dce896cbe5a9870b46b96292d84a6ff1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 Aug 2022 17:21:48 +0200 Subject: [PATCH 14/23] Simplify MoveRelative --- core/src/stages/move_relative.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index f9b2825f..f3435f2a 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -203,10 +203,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning Eigen::Vector3d linear; // linear translation Eigen::Vector3d angular; // angular rotation double linear_norm = 0.0, angular_norm = 0.0; - Eigen::Isometry3d target_eigen; - Eigen::Isometry3d link_pose = - scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success try { // try to extract Twist const geometry_msgs::TwistStamped& target = boost::any_cast(direction); @@ -277,8 +274,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning } COMPUTE: + const Eigen::Isometry3d& link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); + target_eigen = target_eigen * ik_pose_world.inverse() * link_pose; success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); From ec366b26ee095fca2ce6c7a80c72dbcc7907b128 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 Aug 2022 22:28:54 +0200 Subject: [PATCH 15/23] MoveRelative: Correctly compute motion transform The twist motion performs an angular rotation about the given axis _and_ the origin of ik_frame as well as a linear translation. Both transforms are expressed w.r.t. the model frame and thus require left-multiplication to ik_frame's current pose. --- core/src/stages/move_relative.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index f3435f2a..16b7736d 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -237,13 +237,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning angular *= -1.0; } - // compute absolute transform for link + // compute target transform for ik_frame applying motion transform of twist + // linear+angular are expressed w.r.t. model frame and thus we need left-multiplication linear = frame_pose.linear() * linear; angular = frame_pose.linear() * angular; - target_eigen = ik_pose_world; - target_eigen.linear() = - target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular); - target_eigen.translation() += linear; + auto R = Eigen::AngleAxisd(angular_norm, angular); + auto p = ik_pose_world.translation(); + target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world); goto COMPUTE; } catch (const boost::bad_any_cast&) { /* continue with Vector */ } @@ -264,10 +264,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (dir == Interface::BACKWARD) linear *= -1.0; - // compute absolute transform for link + // compute target transform for ik_frame applying delta transform of twist linear = frame_pose.linear() * linear; - target_eigen = ik_pose_world; - target_eigen.translation() += linear; + target_eigen = Eigen::Translation3d(linear) * ik_pose_world; } catch (const boost::bad_any_cast&) { solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); return false; From fd123cc4a77a42a88def59392cb81fa3dc938c3b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 27 Aug 2022 17:09:02 +0200 Subject: [PATCH 16/23] Fix handling of ik_frame in Cartesian path planning The ik_frame should move in a straight-line Cartesian path. However, so far the link frame was following a Cartesian path. --- .../task_constructor/solvers/cartesian_path.h | 4 ++-- .../solvers/joint_interpolation.h | 4 ++-- .../solvers/pipeline_planner.h | 4 ++-- .../solvers/planner_interface.h | 5 +++-- core/src/solvers/cartesian_path.cpp | 8 +++++--- core/src/solvers/joint_interpolation.cpp | 8 ++++---- core/src/solvers/pipeline_planner.cpp | 7 ++++--- core/src/stages/move_relative.cpp | 18 +++++++++--------- core/src/stages/move_to.cpp | 6 +++--- 9 files changed, 34 insertions(+), 30 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 72e63fe3..732111cb 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -66,8 +66,8 @@ public: const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } // namespace solvers diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 6f27c240..ae909120 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -63,8 +63,8 @@ public: const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } // namespace solvers diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 75af734b..3e072e52 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -84,8 +84,8 @@ public: const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; protected: diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 65a7d3ac..60139b3b 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -84,9 +84,10 @@ public: robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; - /// plan trajectory from current robot state to Cartesian target + /// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; }; diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 2e5fac02..9852470f 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -70,11 +70,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, } // reach pose of forward kinematics - return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints); + return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, + timeout, result, path_constraints); } bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double /*timeout*/, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); @@ -96,7 +98,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, moveit::core::MaxEEFStep(props.get("step_size")), moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, - props.get("kinematics_options")); + props.get("kinematics_options"), offset); assert(!trajectory.empty()); // there should be at least the start state result = std::make_shared(sandbox_scene->getRobotModel(), jmg); diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index fc687613..600fd6f0 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -99,9 +99,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, + const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { const auto start_time = std::chrono::steady_clock::now(); @@ -117,7 +117,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr return to->isStateValid(*robot_state, constraints, jmg->getName()); } }; - if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) { + if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) { // TODO(v4hn): planners need a way to add feedback to failing plans // in case of an invalid solution feedback should include unwanted collisions or violated constraints ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target"); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index d2d16295..66d12d5c 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -172,8 +172,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); moveit_msgs::MotionPlanRequest req; @@ -181,7 +182,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co geometry_msgs::PoseStamped target; target.header.frame_id = from->getPlanningFrame(); - target.pose = tf2::toMsg(target_eigen); + target.pose = tf2::toMsg(target_eigen * offset.inverse()); req.goal_constraints.resize(1); req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 16b7736d..0489445b 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -273,22 +273,22 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning } COMPUTE: - const Eigen::Isometry3d& link_pose = scene->getCurrentState().getGlobalLinkTransform(link); - // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * ik_pose_world.inverse() * link_pose; + // offset from link to ik_frame + const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + success = + planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); reached_state->updateLinkTransforms(); - const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link); + const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; double distance = 0.0; if (use_rotation_distance) { - Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); + Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose()); distance = rotation.angle(); } else - distance = (reached_pose.translation() - link_pose.translation()).norm(); + distance = (reached_pose.translation() - ik_pose_world.translation()).norm(); // min_distance reached? if (min_distance > 0.0) { @@ -306,8 +306,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // visualize plan auto ns = props.get("marker_ns"); if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance - visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), link_pose, reached_pose, linear, - distance); + visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose, + linear, distance); } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 5611475c..0ce8ea11 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -228,11 +228,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP add_frame(target, "target frame"); add_frame(ik_pose_world, "ik frame"); - // transform target pose such that ik frame will reach there if link does - target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); + // offset from link to ik_frame + Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; // plan to Cartesian target - success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints); + success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); } // store result From 32d3454c1fdaa698e5c96340ca4b68f9aa67ad27 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 28 Aug 2022 10:22:37 +0200 Subject: [PATCH 17/23] Fix getRobotTipForFrame() When passing the root frame, getRigidlyConnectedParentLinkModel() returns a nullptr for robot_link, causing a segfault. Actually, we don't need to use that method at all. We just need to find the robot_link of an associated body. --- core/src/utils.cpp | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 8b7c484b..37669277 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -69,23 +69,27 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); } else { auto ik_pose_msg = boost::any_cast(property.value()); - if (ik_pose_msg.header.frame_id.empty()) { - if (!(robot_link = get_tip())) { - solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); - return false; - } - tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); - tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame; - } else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) { - robot_link = scene.getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id); - tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); - tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; - } else { + tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); + + robot_link = nullptr; + bool found = false; + auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found); + if (!found && !ik_pose_msg.header.frame_id.empty()) { std::stringstream ss; - ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; + ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'"; solution.markAsFailure(ss.str()); return false; } + if (!robot_link) + robot_link = get_tip(); + if (!robot_link) { + solution.markAsFailure("ik_frame doesn't specify a link frame"); + return false; + } else if (!found) { // use robot link's frame as reference by default + ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); + } + + tip_in_global_frame = ref_frame * tip_in_global_frame; } return true; From d66b2262edd3f05429c6e49068c547bdac903ebb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 28 Aug 2022 11:13:23 +0200 Subject: [PATCH 18/23] Rename variables in visualizePlan() - link_pose -> start_pose - pos_link -> pos_start --- core/src/stages/move_relative.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 0489445b..30af9aa5 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -104,8 +104,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c return false; } +// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance static void visualizePlan(std::deque& markers, Interface::Direction dir, bool success, - const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose, + const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) { double linear_norm = linear.norm(); @@ -114,7 +115,7 @@ static void visualizePlan(std::deque& markers, Inter auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()); // link position before planning; reached link position after planning; target link position - Eigen::Vector3d pos_link = link_pose.translation(); + Eigen::Vector3d pos_start = start_pose.translation(); Eigen::Vector3d pos_reached = reached_pose.translation(); Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0); @@ -124,7 +125,7 @@ static void visualizePlan(std::deque& markers, Inter if (dir == Interface::FORWARD) { if (success) { // valid part: green arrow - rviz_marker_tools::makeArrow(m, pos_link, pos_reached, 0.1 * linear_norm); + rviz_marker_tools::makeArrow(m, pos_start, pos_reached, 0.1 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); } else { @@ -139,14 +140,14 @@ static void visualizePlan(std::deque& markers, Inter rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); // position half-way between pos_link and pos_reached - m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) }); + m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_start + pos_reached) }); m.pose.orientation = tf2::toMsg(quat_cylinder); markers.push_back(m); } } else { // valid part: green arrow // head length according to above comment - rviz_marker_tools::makeArrow(m, pos_reached, pos_link, 0.1 * linear_norm, 0.23 * linear_norm); + rviz_marker_tools::makeArrow(m, pos_reached, pos_start, 0.1 * linear_norm, 0.23 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); if (!success) { From 0c9524930af1c0579a2cef64f82e63a93ecebc47 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 1 Nov 2022 21:24:07 +0100 Subject: [PATCH 19/23] CI: stricter warnings --- .github/workflows/ci.yaml | 10 ++++------ core/src/container.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 1c303e77..b5608fed 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -16,18 +16,16 @@ jobs: env: - IMAGE: master-source TARGET_CMAKE_ARGS: >- - -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=" - -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls - -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter" + -DCMAKE_BUILD_TYPE=Release + -DCMAKE_CXX_FLAGS="-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - IMAGE: noetic-source NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: noetic-source CLANG_TIDY: true TARGET_CMAKE_ARGS: >- - -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=" - -Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls - -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy" + -DCMAKE_BUILD_TYPE=Release + -DCMAKE_CXX_FLAGS="-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" - IMAGE: noetic-source NAME: asan DOCKER_RUN_OPTS: >- diff --git a/core/src/container.cpp b/core/src/container.cpp index d7e4b372..503627c5 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -56,8 +56,10 @@ namespace moveit { namespace task_constructor { // for debugging of how children interfaces evolve over time -static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, - std::ostream& os = std::cerr) { +__attribute__((unused)) // silent unused-function warning +static void +printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator, + std::ostream& os = std::cerr) { static unsigned int id = 0; const unsigned int width = 10; // indentation of name os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' '; From 7ff2f70b669bf6a82dbec337a4cd06d1760617b4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 2 Nov 2022 08:13:28 +0100 Subject: [PATCH 20/23] CI: make clang-tidy checks pedantic --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index b5608fed..ded2a005 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -22,7 +22,7 @@ jobs: NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: noetic-source - CLANG_TIDY: true + CLANG_TIDY: pedantic TARGET_CMAKE_ARGS: >- -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls" From 7d5f9fe6e39fe714c2c69fdd7a73d0d37f28a6c1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 2 Nov 2022 12:25:03 +0100 Subject: [PATCH 21/23] Fix clang-tidy warnings --- .../task_constructor/stages/fixed_state.h | 2 +- .../include/moveit/task_constructor/storage.h | 2 +- core/src/stage.cpp | 4 +-- core/src/stages/compute_ik.cpp | 2 +- core/src/stages/fixed_state.cpp | 2 +- core/test/test_move_relative.cpp | 8 +++--- core/test/test_move_to.cpp | 27 ++++++++++--------- core/test/test_stage.cpp | 2 +- .../motion_planning_tasks/src/task_panel.cpp | 2 +- 9 files changed, 26 insertions(+), 25 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/fixed_state.h b/core/include/moveit/task_constructor/stages/fixed_state.h index 2ace2726..adb3bb1c 100644 --- a/core/include/moveit/task_constructor/stages/fixed_state.h +++ b/core/include/moveit/task_constructor/stages/fixed_state.h @@ -48,7 +48,7 @@ namespace stages { class FixedState : public Generator { public: - FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr); + FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr scene = nullptr); void setState(const planning_scene::PlanningScenePtr& scene); void reset() override; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index ed8ef9d2..c11bbd79 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -248,7 +248,7 @@ private: std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio); std::ostream& operator<<(std::ostream& os, const Interface& interface); -std::ostream& operator<<(std::ostream& os, Interface::Direction); +std::ostream& operator<<(std::ostream& os, Interface::Direction dir); /// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found. template diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 60315e57..6b2ff546 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -125,8 +125,8 @@ StagePrivate& StagePrivate::operator=(StagePrivate&& other) { prev_ends_ = std::move(other.prev_ends_); next_starts_ = std::move(other.next_starts_); - parent_ = std::move(other.parent_); - it_ = std::move(other.it_); + parent_ = other.parent_; + it_ = other.it_; other.unparent(); return *this; diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 2b8ece2a..1f793f77 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -373,7 +373,7 @@ void ComputeIK::compute() { req.max_contacts = 1; scene->checkCollision(req, res, *state); solution.feasible = ignore_collisions || !res.collision; - if (res.contacts.size() > 0) { + if (!res.contacts.empty()) { solution.contact = res.contacts.begin()->second.front(); } return solution.feasible; diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index 9c4d99b0..0c8ebaad 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -44,7 +44,7 @@ namespace task_constructor { namespace stages { FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) - : Generator(name), scene_(scene) { + : Generator(name), scene_(std::move(scene)) { properties().declare("ignore_collisions", false); setCostTerm(std::make_unique(0.0)); } diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 37c12705..43caa12f 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -113,9 +113,9 @@ TEST_F(PandaMoveRelative, cartesianCircular) { } TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { - const std::string ATTACHED_OBJECT{ "attached_object" }; - scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); - move->setIKFrame(ATTACHED_OBJECT); + const std::string attached_object{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); + move->setIKFrame(attached_object); move->setDirection([] { geometry_msgs::TwistStamped twist; @@ -125,7 +125,7 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { }()); ASSERT_TRUE(t.plan()) << "Failed to plan"; - EXPECT_CONST_POSITION(move->solutions().front(), ATTACHED_OBJECT); + EXPECT_CONST_POSITION(move->solutions().front(), attached_object); } int main(int argc, char** argv) { diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 83d6b076..8e7ac023 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -72,7 +72,8 @@ TEST_F(PandaMoveTo, stateTarget) { EXPECT_ONE_SOLUTION; } -geometry_msgs::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) { +geometry_msgs::PoseStamped getFramePoseOfNamedState(RobotState state, const std::string& pose, + const std::string& frame) { state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose); auto frame_eigen{ state.getFrameTransform(frame) }; geometry_msgs::PoseStamped p; @@ -98,9 +99,9 @@ TEST_F(PandaMoveTo, poseTarget) { } TEST_F(PandaMoveTo, poseIKFrameLinkTarget) { - const std::string IK_FRAME{ "panda_hand" }; - move_to->setIKFrame(IK_FRAME); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + const std::string ik_frame{ "panda_hand" }; + move_to->setIKFrame(ik_frame); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame)); EXPECT_ONE_SOLUTION; } @@ -129,22 +130,22 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) } TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) { - const std::string ATTACHED_OBJECT{ "attached_object" }; - scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + const std::string attached_object{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); - move_to->setIKFrame(ATTACHED_OBJECT); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT)); + move_to->setIKFrame(attached_object); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", attached_object)); EXPECT_ONE_SOLUTION; } TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { - const std::string ATTACHED_OBJECT{ "attached_object" }; - const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; + const std::string attached_object{ "attached_object" }; + const std::string ik_frame{ attached_object + "/subframe" }; - scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object)); - move_to->setIKFrame(IK_FRAME); - move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + move_to->setIKFrame(ik_frame); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame)); EXPECT_ONE_SOLUTION; } diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 180bc70d..01284a96 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -19,7 +19,7 @@ struct StandaloneGeneratorMockup : public GeneratorMockup InterfacePtr next; StandaloneGeneratorMockup(std::initializer_list&& costs) - : StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {} + : StandaloneGeneratorMockup{ PredefinedCosts{ costs, true } } {} StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true }) : GeneratorMockup{ std::move(costs) } { diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index b73e2e15..f571ccb0 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -172,7 +172,7 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) { tool_buttons_group = new QButtonGroup(panel); tool_buttons_group->setExclusive(true); button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); - button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages")); + button_show_stage_dock_widget->setToolTip("Show available stages"); property_root = new rviz::Property("Global Settings"); } From f0a57850748edfc26a8ed90df107caf25b23ee31 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 2 Nov 2022 17:20:52 +0100 Subject: [PATCH 22/23] Disable clang-tidy checks generating false positives --- .clang-tidy | 2 ++ visualization/motion_planning_tasks/test/test_task_model.cpp | 2 ++ .../motion_planning_tasks/utils/flat_merge_proxy_model.cpp | 1 + .../motion_planning_tasks/utils/tree_merge_proxy_model.cpp | 1 + 4 files changed, 6 insertions(+) diff --git a/.clang-tidy b/.clang-tidy index 9589fc7d..181fbf69 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,5 +1,7 @@ --- Checks: 'performance-*, + -performance-noexcept-move-constructor, + -clang-analyzer-core.CallAndMessage, llvm-namespace-comment, modernize-redundant-void-arg, modernize-use-nullptr, diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 8060235c..5d8f2f78 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -234,6 +234,8 @@ TEST_F(TaskListModelTest, deletion) { int main(int argc, char** argv) { ros::init(argc, argv, "test_task_model"); QCoreApplication app(argc, argv); + // https://bugs.llvm.org/show_bug.cgi?id=40367 + // NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks) QTimer::singleShot(0, [&]() { ::testing::InitGoogleTest(&argc, argv); auto testResult = RUN_ALL_TESTS(); diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp index a8453d8a..343942a8 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp @@ -467,6 +467,7 @@ std::pair FlatMergeProxyModel::getModel(const const QModelIndex& src_index = d_ptr->mapToSource(index, data); Q_ASSERT(data); + // NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker) return std::make_pair(data->model_, src_index); } diff --git a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp index f57023ed..75e1ad30 100644 --- a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp @@ -440,6 +440,7 @@ std::pair TreeMergeProxyModel::getModel(const const QModelIndex& src_index = d_ptr->mapToSource(index, data); Q_ASSERT(data); + // NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker) return std::make_pair(data->model_, src_index); } From 38cc5613c05df4d78d7ed44203857e2aea28266c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 2 Nov 2022 17:26:57 +0100 Subject: [PATCH 23/23] Disable identifier naming checks --- .clang-tidy | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 181fbf69..680317d8 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -14,7 +14,6 @@ Checks: 'performance-*, readability-redundant-string-cstr, readability-simplify-boolean-expr, readability-container-size-empty, - readability-identifier-naming, ' HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h' AnalyzeTemporaryDtors: false @@ -27,14 +26,14 @@ CheckOptions: value: '2' # type names - key: readability-identifier-naming.ClassCase - value: aNy_CasE # CamelCase + value: CamelCase - key: readability-identifier-naming.EnumCase value: CamelCase - key: readability-identifier-naming.UnionCase value: CamelCase # method names - key: readability-identifier-naming.MethodCase - value: aNy_CasE # camelBack + value: camelBack # variable names - key: readability-identifier-naming.VariableCase value: lower_case