From 67fe4faa30ec3d85ea9b0c65ec26c951e8b5c35a Mon Sep 17 00:00:00 2001 From: j-kuehn <57101356+j-kuehn@users.noreply.github.com> Date: Tue, 25 May 2021 10:40:32 +0200 Subject: [PATCH 01/17] allow to consider specific joints only in cost terms --- .../moveit/task_constructor/cost_terms.h | 7 ++++-- core/src/cost_terms.cpp | 22 ++++++++++++++++--- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index b8fcb369..e9d8d502 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -115,9 +115,12 @@ public: /// trajectory length (interpolated between waypoints) class PathLength : public TrajectoryCostTerm { - // TODO(v4hn): allow to consider specific joints only public: - double operator()(const SubTrajectory& s, std::string& comment) const override; + PathLength() = default; + PathLength(std::vector j) : joints{ std::move(j) } {}; + double operator()(const SubTrajectory&, std::string&) const override; + + std::vector joints; }; /// execution duration of the whole trajectory diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 81be1a72..7384de48 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -110,12 +110,28 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const { const auto& traj = s.trajectory(); - if (traj == nullptr) + if (traj == nullptr || traj->getWayPointCount() == 0) return 0.0; + std::vector joint_models; + joint_models.reserve(joints.size()); + const auto& first_waypoint = traj->getWayPoint(0); + for (auto& joint : joints) { + joint_models.push_back(first_waypoint.getJointModel(joint)); + } + double path_length{ 0.0 }; - for (size_t i = 1; i < traj->getWayPointCount(); ++i) - path_length += traj->getWayPoint(i - 1).distance(traj->getWayPoint(i)); + for (size_t i = 1; i < traj->getWayPointCount(); ++i) { + auto& last = traj->getWayPoint(i - 1); + auto& curr = traj->getWayPoint(i); + if (joints.empty()) { + path_length += last.distance(curr); + } else { + for (const auto& model : joint_models) { + path_length += last.distance(curr, model); + } + } + } return path_length; } From f5022cc1d4ca3a73d3e9703a7bf6548b862830ed Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 19 May 2021 17:36:55 +0200 Subject: [PATCH 02/17] GHA: check for warnings + address sanitizer --- .github/workflows/ci.yaml | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 2b2e14cb..364bb0dc 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -22,9 +22,14 @@ jobs: CXX: clang++ - 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 -Wno-unused-but-set-parameter + - IMAGE: noetic-source + NAME: asan + TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1" env: CATKIN_LINT: true - CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy" UNDERLAY: /root/ws_moveit/install DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master" CCACHE_DIR: ${{ github.workspace }}/.ccache From f070247a252d649f6708ba01b76ab1d392622be2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 27 May 2021 21:13:21 +0200 Subject: [PATCH 03/17] Fix compiler warnings --- core/include/moveit/task_constructor/task.h | 1 + visualization/motion_planning_tasks/src/task_panel.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 4c4e9eee..eb2b8e16 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -151,6 +151,7 @@ protected: void onNewSolution(const SolutionBase& s) override; private: + using WrapperBase::init; }; inline std::ostream& operator<<(std::ostream& os, const Task& task) { diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 885543cc..7b37a602 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -158,6 +158,7 @@ void TaskPanel::request(rviz::WindowManagerInterface* window_manager) { QDockWidget* dock = vis_frame->addPanelByName("Motion Planning Tasks", "moveit_task_constructor/Motion Planning Tasks", Qt::LeftDockWidgetArea, true /* floating */); + Q_UNUSED(dock); assert(dock->widget() == SINGLETON); } From 58b1a020b57653ae429e41e5da1401c00e63ca32 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 27 May 2021 14:19:38 +0200 Subject: [PATCH 04/17] Fix memory leak in unit test --- core/test/test_cost_queue.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index 86888c04..c6b56823 100644 --- a/core/test/test_cost_queue.cpp +++ b/core/test/test_cost_queue.cpp @@ -16,7 +16,10 @@ int create(int cost) { } template <> int* create(int cost) { - return new int(cost); + // return new int(cost), but store values for clean memory management + static std::list storage; + storage.push_back(cost); + return &storage.back(); } template <> mtc::SolutionBasePtr create(int cost) { From 059a939203355da2aadd5bc8346f6aebb2ecc985 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 27 May 2021 14:43:20 +0200 Subject: [PATCH 05/17] Fix test: correctly reset Interface --- core/test/test_cost_terms.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index a6a87d55..2f1cd1af 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -112,6 +112,7 @@ public: auto impl{ this->pimpl() }; this->reset(); + dummy->clear(); state_start.reset(); state_end.reset(); From ff4eb4edf15bf29c215da5c110bf16a0455aac3b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Fri, 28 May 2021 00:44:27 +0200 Subject: [PATCH 06/17] Run pick+place demo as a unit test (#254) --- demo/CMakeLists.txt | 10 +- .../pick_place_task.h | 21 ++- demo/package.xml | 15 +- demo/src/pick_place_demo.cpp | 78 ++------- demo/src/pick_place_task.cpp | 150 +++++++++++++----- demo/test/CMakeLists.txt | 13 ++ demo/test/pick_place.test | 14 ++ demo/test/pick_place_test.cpp | 39 +++++ 8 files changed, 213 insertions(+), 127 deletions(-) create mode 100644 demo/test/CMakeLists.txt create mode 100644 demo/test/pick_place.test create mode 100644 demo/test/pick_place_test.cpp diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 89d5240a..81b83565 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -11,7 +11,9 @@ find_package(catkin REQUIRED COMPONENTS rosparam_shortcuts ) -catkin_package() +catkin_package( + CATKIN_DEPENDS roscpp +) include_directories( include @@ -24,7 +26,9 @@ target_link_libraries(cartesian ${catkin_LIBRARIES}) add_executable(modular src/modular.cpp) target_link_libraries(modular ${catkin_LIBRARIES}) -add_executable(pick_place_demo src/pick_place_demo.cpp src/pick_place_task.cpp) +add_library(pick_place_task OBJECT src/pick_place_task.cpp) + +add_executable(pick_place_demo src/pick_place_demo.cpp $) target_link_libraries(pick_place_demo ${catkin_LIBRARIES}) add_executable(alternative_path_costs src/alternative_path_costs.cpp) @@ -42,3 +46,5 @@ install(TARGETS cartesian modular pick_place_demo install(DIRECTORY launch config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +add_subdirectory(test) diff --git a/demo/include/moveit_task_constructor_demo/pick_place_task.h b/demo/include/moveit_task_constructor_demo/pick_place_task.h index d45d7ef8..5301fb6d 100644 --- a/demo/include/moveit_task_constructor_demo/pick_place_task.h +++ b/demo/include/moveit_task_constructor_demo/pick_place_task.h @@ -58,9 +58,6 @@ #include #include -#include -#include - #include #pragma once @@ -68,22 +65,27 @@ namespace moveit_task_constructor_demo { using namespace moveit::task_constructor; +// prepare a demo environment from ROS parameters under pnh +void setupDemoScene(ros::NodeHandle& pnh); + class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); + PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh); ~PickPlaceTask() = default; - void loadParameters(); - - void init(); + bool init(); bool plan(); bool execute(); private: - ros::NodeHandle nh_; + void loadParameters(); + + static constexpr char LOGNAME[]{ "pick_place_task" }; + + ros::NodeHandle pnh_; std::string task_name_; moveit::task_constructor::TaskPtr task_; @@ -107,9 +109,6 @@ private: std::string hand_close_pose_; std::string arm_home_pose_; - // Execution - actionlib::SimpleActionClient execute_; - // Pick metrics Eigen::Isometry3d grasp_frame_transform_; double approach_object_min_dist_; diff --git a/demo/package.xml b/demo/package.xml index a7700225..9957ac90 100644 --- a/demo/package.xml +++ b/demo/package.xml @@ -11,10 +11,17 @@ BSD catkin - moveit_task_constructor_core - moveit_ros_planning_interface - moveit_core + + roscpp rosparam_shortcuts - roscpp + + moveit_core + moveit_ros_planning_interface + + moveit_task_constructor_core + + moveit_task_constructor_capabilities moveit_resources_panda_moveit_config + + rostest diff --git a/demo/src/pick_place_demo.cpp b/demo/src/pick_place_demo.cpp index 4643c83e..a14c425e 100644 --- a/demo/src/pick_place_demo.cpp +++ b/demo/src/pick_place_demo.cpp @@ -40,83 +40,25 @@ // MTC pick/place demo implementation #include -#include -#include -#include -#include - constexpr char LOGNAME[] = "moveit_task_constructor_demo"; -void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { - if (!psi.applyCollisionObject(object)) - throw std::runtime_error("Failed to spawn object: " + object.id); -} - -moveit_msgs::CollisionObject createTable() { - ros::NodeHandle pnh("~"); - std::string table_name, table_reference_frame; - std::vector table_dimensions; - geometry_msgs::Pose pose; - std::size_t errors = 0; - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, errors); - - moveit_msgs::CollisionObject object; - object.id = table_name; - object.header.frame_id = table_reference_frame; - object.primitives.resize(1); - object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - object.primitives[0].dimensions = table_dimensions; - pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world - object.primitive_poses.push_back(pose); - return object; -} - -moveit_msgs::CollisionObject createObject() { - ros::NodeHandle pnh("~"); - std::string object_name, object_reference_frame; - std::vector object_dimensions; - geometry_msgs::Pose pose; - std::size_t error = 0; - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); - error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); - rosparam_shortcuts::shutdownIfError(LOGNAME, error); - - moveit_msgs::CollisionObject object; - object.id = object_name; - object.header.frame_id = object_reference_frame; - object.primitives.resize(1); - object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; - object.primitives[0].dimensions = object_dimensions; - pose.position.z += 0.5 * object_dimensions[0]; - object.primitive_poses.push_back(pose); - return object; -} - int main(int argc, char** argv) { - ROS_INFO_NAMED(LOGNAME, "Init node"); ros::init(argc, argv, "mtc_tutorial"); - ros::NodeHandle nh; + ros::NodeHandle nh, pnh("~"); + + // Handle Task introspection requests from RViz & feedback during execution ros::AsyncSpinner spinner(1); spinner.start(); - // Add table and object to planning scene - ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service - moveit::planning_interface::PlanningSceneInterface psi; - ros::NodeHandle pnh("~"); - if (pnh.param("spawn_table", true)) - spawnObject(psi, createTable()); - spawnObject(psi, createObject()); + moveit_task_constructor_demo::setupDemoScene(pnh); // Construct and run pick/place task - moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh); - pick_place_task.loadParameters(); - pick_place_task.init(); + moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh); + if (!pick_place_task.init()) { + ROS_INFO_NAMED(LOGNAME, "Initialization failed"); + return 1; + } + if (pick_place_task.plan()) { ROS_INFO_NAMED(LOGNAME, "Planning succeded"); if (pnh.param("execute", false)) { diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index 74b08f8a..a980007d 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -38,9 +38,72 @@ #include namespace moveit_task_constructor_demo { -constexpr char LOGNAME[] = "pick_place_task"; -PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) - : nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {} + +constexpr char LOGNAME[] = "moveit_task_constructor_demo"; +constexpr char PickPlaceTask::LOGNAME[]; + +void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { + if (!psi.applyCollisionObject(object)) + throw std::runtime_error("Failed to spawn object: " + object.id); +} + +moveit_msgs::CollisionObject createTable(ros::NodeHandle& pnh) { + std::string table_name, table_reference_frame; + std::vector table_dimensions; + geometry_msgs::Pose pose; + std::size_t errors = 0; + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); + errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, errors); + + moveit_msgs::CollisionObject object; + object.id = table_name; + object.header.frame_id = table_reference_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + object.primitives[0].dimensions = table_dimensions; + pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world + object.primitive_poses.push_back(pose); + return object; +} + +moveit_msgs::CollisionObject createObject(ros::NodeHandle& pnh) { + std::string object_name, object_reference_frame; + std::vector object_dimensions; + geometry_msgs::Pose pose; + std::size_t error = 0; + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); + error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); + rosparam_shortcuts::shutdownIfError(LOGNAME, error); + + moveit_msgs::CollisionObject object; + object.id = object_name; + object.header.frame_id = object_reference_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; + object.primitives[0].dimensions = object_dimensions; + pose.position.z += 0.5 * object_dimensions[0]; + object.primitive_poses.push_back(pose); + return object; +} + +void setupDemoScene(ros::NodeHandle& pnh) { + // Add table and object to planning scene + ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service + moveit::planning_interface::PlanningSceneInterface psi; + if (pnh.param("spawn_table", true)) + spawnObject(psi, createTable(pnh)); + spawnObject(psi, createObject(pnh)); +} + +PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh) + : pnh_(pnh), task_name_(task_name) { + loadParameters(); +} void PickPlaceTask::loadParameters() { /**************************************************** @@ -49,45 +112,44 @@ void PickPlaceTask::loadParameters() { * * ***************************************************/ ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); - ros::NodeHandle pnh("~"); // Planning group properties size_t errors = 0; - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_); // Predefined pose targets - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_); // Target object - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_); support_surfaces_ = { surface_link_ }; // Pick/Place metrics - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); - errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_); + errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_); rosparam_shortcuts::shutdownIfError(LOGNAME, errors); } -void PickPlaceTask::init() { +bool PickPlaceTask::init() { ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); const std::string object = object_name_; // Reset ROS introspection before constructing the new object - // TODO(henningkayser): verify this is a bug, fix if possible + // TODO(v4hn): global storage for Introspection services to enable one-liner task_.reset(); task_.reset(new moveit::task_constructor::Task()); @@ -414,36 +476,40 @@ void PickPlaceTask::init() { stage->restrictDirection(stages::MoveTo::FORWARD); t.add(std::move(stage)); } -} - -bool PickPlaceTask::plan() { - ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); - ros::NodeHandle pnh("~"); - int max_solutions = pnh.param("max_solutions", 10); + // prepare Task structure for planning try { - task_->plan(max_solutions); + t.init(); } catch (InitStageException& e) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); return false; } - if (task_->numSolutions() == 0) { - ROS_ERROR_NAMED(LOGNAME, "Planning failed"); - return false; - } + return true; } +bool PickPlaceTask::plan() { + ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); + int max_solutions = pnh_.param("max_solutions", 10); + + return task_->plan(max_solutions); +} + bool PickPlaceTask::execute() { ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; - task_->solutions().front()->fillMessage(execute_goal.solution); - execute_.sendGoal(execute_goal); - execute_.waitForResult(); - moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code; + moveit_msgs::MoveItErrorCodes execute_result; + + execute_result = task_->execute(*task_->solutions().front()); + // // If you want to inspect the goal message, use this instead: + // actionlib::SimpleActionClient + // execute("execute_task_solution", true); execute.waitForServer(); + // moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; + // task_->solutions().front()->fillMessage(execute_goal.solution); + // execute.sendGoalAndWait(execute_goal); + // execute_result = execute.getResult()->error_code; if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString()); + ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val); return false; } diff --git a/demo/test/CMakeLists.txt b/demo/test/CMakeLists.txt new file mode 100644 index 00000000..39b87bb2 --- /dev/null +++ b/demo/test/CMakeLists.txt @@ -0,0 +1,13 @@ +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_executable(pick_place_test pick_place_test.cpp $) + target_link_libraries(pick_place_test ${catkin_LIBRARIES} gtest_main) + + add_rostest(pick_place.test) +endif() diff --git a/demo/test/pick_place.test b/demo/test/pick_place.test new file mode 100644 index 00000000..88c36eec --- /dev/null +++ b/demo/test/pick_place.test @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/demo/test/pick_place_test.cpp b/demo/test/pick_place_test.cpp new file mode 100644 index 00000000..0e810257 --- /dev/null +++ b/demo/test/pick_place_test.cpp @@ -0,0 +1,39 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +using namespace moveit::task_constructor; + +TEST(PickPlaceDemo, run) { + ros::NodeHandle nh, pnh("~"); + + moveit_task_constructor_demo::setupDemoScene(pnh); + + // Construct and run pick/place task + moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh); + + ASSERT_TRUE(pick_place_task.init()); + + ASSERT_TRUE(pick_place_task.plan()); + ASSERT_TRUE(pick_place_task.execute()); +} + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "pick_place_test"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + return RUN_ALL_TESTS(); +} From 84d668b0d5d56c7ba6ce09b919e3763f6c87ebf9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Sat, 29 May 2021 00:38:02 +0200 Subject: [PATCH 07/17] Split ClassLoader plugin code from main libraries (#271) to work around https://github.com/ros/class_loader/issues/131 --- core/CMakeLists.txt | 1 + core/motion_planning_stages_plugin_description.xml | 2 +- core/src/stages/CMakeLists.txt | 7 ++++++- core/src/stages/current_state.cpp | 4 ---- core/src/stages/plugins.cpp | 7 +++++++ 5 files changed, 15 insertions(+), 6 deletions(-) create mode 100644 core/src/stages/plugins.cpp diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 7032b57d..d16658c3 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -18,6 +18,7 @@ catkin_package( LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_stages + ${PROJECT_NAME}_stage_plugins INCLUDE_DIRS include CATKIN_DEPENDS diff --git a/core/motion_planning_stages_plugin_description.xml b/core/motion_planning_stages_plugin_description.xml index 75af7442..389c11ff 100644 --- a/core/motion_planning_stages_plugin_description.xml +++ b/core/motion_planning_stages_plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 857a4977..2becaa63 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -41,6 +41,11 @@ add_library(${PROJECT_NAME}_stages ) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) -install(TARGETS ${PROJECT_NAME}_stages +add_library(${PROJECT_NAME}_stage_plugins + plugins.cpp +) +target_link_libraries(${PROJECT_NAME}_stage_plugins ${PROJECT_NAME}_stages ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME}_stages ${PROJECT_NAME}_stage_plugins ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index 51c5abc9..822dc5c3 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -96,7 +96,3 @@ void CurrentState::compute() { } // namespace stages } // namespace task_constructor } // namespace moveit - -/// register plugin -#include -PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::CurrentState, moveit::task_constructor::Stage) diff --git a/core/src/stages/plugins.cpp b/core/src/stages/plugins.cpp new file mode 100644 index 00000000..9b9864f8 --- /dev/null +++ b/core/src/stages/plugins.cpp @@ -0,0 +1,7 @@ +#include + +#include + +/// register plugins to use with ClassLoader + +PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::CurrentState, moveit::task_constructor::Stage) From dce740fdfe1ed98d68ef3c40599dd59206d32ad6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 28 May 2021 16:50:17 +0200 Subject: [PATCH 08/17] CI: fixup asan job Need to preload libasan.so.5 for move_group node, because it loads a sanitized plugin. --- .github/workflows/ci.yaml | 1 + demo/test/pick_place.test | 1 + 2 files changed, 2 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 364bb0dc..bda2f42c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -27,6 +27,7 @@ jobs: -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter - IMAGE: noetic-source NAME: asan + DOCKER_RUN_OPTS: "-e PRELOAD=libasan.so.5" TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1" env: CATKIN_LINT: true diff --git a/demo/test/pick_place.test b/demo/test/pick_place.test index 88c36eec..89a500bd 100644 --- a/demo/test/pick_place.test +++ b/demo/test/pick_place.test @@ -3,6 +3,7 @@ + From 437cc550f2a68895de56f025e2507770bff2faca Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 28 May 2021 23:40:42 +0200 Subject: [PATCH 09/17] Add missing semicolons after cleanup of MoveIt --- .../moveit/task_constructor/container_p.h | 4 ++-- .../moveit/task_constructor/introspection.h | 4 ++-- .../moveit/task_constructor/marker_tools.h | 6 +++--- .../task_constructor/solvers/cartesian_path.h | 2 +- .../solvers/joint_interpolation.h | 2 +- .../solvers/pipeline_planner.h | 4 ++-- .../solvers/planner_interface.h | 12 ++++++------ core/include/moveit/task_constructor/stage.h | 12 ++++++------ .../task_constructor/stages/compute_ik.h | 4 ++-- .../moveit/task_constructor/stages/connect.h | 2 +- .../stages/modify_planning_scene.h | 2 +- .../moveit/task_constructor/stages/pick.h | 2 +- .../task_constructor/stages/predicate_filter.h | 2 +- .../task_constructor/stages/simple_grasp.h | 2 +- core/include/moveit/task_constructor/storage.h | 18 +++++++++--------- core/include/moveit/task_constructor/task.h | 10 +++++----- core/include/moveit/task_constructor/task_p.h | 2 +- core/test/models.h | 2 +- .../src/meta_task_list_model.h | 6 +++--- .../motion_planning_tasks/src/task_display.h | 6 +++--- .../src/task_list_model.h | 4 ++-- .../motion_planning_tasks/src/task_panel.h | 4 ++-- .../visualization_tools/display_solution.h | 10 +++++----- .../visualization_tools/marker_visualization.h | 6 +++--- .../task_solution_visualization.h | 14 +++++++------- 25 files changed, 71 insertions(+), 71 deletions(-) diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 9e374d95..d62b9fbc 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -51,8 +51,8 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(JointModelGroup) -MOVEIT_CLASS_FORWARD(RobotState) +MOVEIT_CLASS_FORWARD(JointModelGroup); +MOVEIT_CLASS_FORWARD(RobotState); } // namespace core } // namespace moveit diff --git a/core/include/moveit/task_constructor/introspection.h b/core/include/moveit/task_constructor/introspection.h index 1e52548c..d57c05ce 100644 --- a/core/include/moveit/task_constructor/introspection.h +++ b/core/include/moveit/task_constructor/introspection.h @@ -52,8 +52,8 @@ namespace moveit { namespace task_constructor { -MOVEIT_CLASS_FORWARD(Stage) -MOVEIT_CLASS_FORWARD(SolutionBase) +MOVEIT_CLASS_FORWARD(Stage); +MOVEIT_CLASS_FORWARD(SolutionBase); class TaskPrivate; class IntrospectionPrivate; diff --git a/core/include/moveit/task_constructor/marker_tools.h b/core/include/moveit/task_constructor/marker_tools.h index f4d76282..5c47c067 100644 --- a/core/include/moveit/task_constructor/marker_tools.h +++ b/core/include/moveit/task_constructor/marker_tools.h @@ -6,12 +6,12 @@ #include namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene) +MOVEIT_CLASS_FORWARD(PlanningScene); } namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotState) -MOVEIT_CLASS_FORWARD(LinkModel) +MOVEIT_CLASS_FORWARD(RobotState); +MOVEIT_CLASS_FORWARD(LinkModel); } // namespace core } // namespace moveit diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index d51fe14b..72e63fe3 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -44,7 +44,7 @@ namespace moveit { namespace task_constructor { namespace solvers { -MOVEIT_CLASS_FORWARD(CartesianPath) +MOVEIT_CLASS_FORWARD(CartesianPath); /** Use MoveIt's computeCartesianPath() to generate a straigh-line path between to scenes */ class CartesianPath : public PlannerInterface diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index b6ec022a..6f27c240 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -45,7 +45,7 @@ namespace moveit { namespace task_constructor { namespace solvers { -MOVEIT_CLASS_FORWARD(JointInterpolationPlanner) +MOVEIT_CLASS_FORWARD(JointInterpolationPlanner); /** Interpolate a trajectory between states in joint space * diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 015c5f41..75af734b 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -42,14 +42,14 @@ #include namespace planning_pipeline { -MOVEIT_CLASS_FORWARD(PlanningPipeline) +MOVEIT_CLASS_FORWARD(PlanningPipeline); } namespace moveit { namespace task_constructor { namespace solvers { -MOVEIT_CLASS_FORWARD(PipelinePlanner) +MOVEIT_CLASS_FORWARD(PipelinePlanner); /** Use MoveIt's PlanningPipeline to plan a trajectory between to scenes */ class PipelinePlanner : public PlannerInterface diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 45a033e4..65a7d3ac 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -44,16 +44,16 @@ #include namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene) +MOVEIT_CLASS_FORWARD(PlanningScene); } namespace robot_trajectory { -MOVEIT_CLASS_FORWARD(RobotTrajectory) +MOVEIT_CLASS_FORWARD(RobotTrajectory); } namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(LinkModel) -MOVEIT_CLASS_FORWARD(RobotModel) -MOVEIT_CLASS_FORWARD(JointModelGroup) +MOVEIT_CLASS_FORWARD(LinkModel); +MOVEIT_CLASS_FORWARD(RobotModel); +MOVEIT_CLASS_FORWARD(JointModelGroup); } // namespace core } // namespace moveit @@ -61,7 +61,7 @@ namespace moveit { namespace task_constructor { namespace solvers { -MOVEIT_CLASS_FORWARD(PlannerInterface) +MOVEIT_CLASS_FORWARD(PlannerInterface); class PlannerInterface { // these properties take precedence over stage properties diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 11187765..79c27837 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -52,20 +52,20 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotModel) +MOVEIT_CLASS_FORWARD(RobotModel); } } // namespace moveit namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene) +MOVEIT_CLASS_FORWARD(PlanningScene); } namespace planning_pipeline { -MOVEIT_CLASS_FORWARD(PlanningPipeline) +MOVEIT_CLASS_FORWARD(PlanningPipeline); } namespace robot_trajectory { -MOVEIT_CLASS_FORWARD(RobotTrajectory) +MOVEIT_CLASS_FORWARD(RobotTrajectory); } namespace moveit { @@ -108,8 +108,8 @@ constexpr InterfaceFlags UNKNOWN; constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END }); constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START }); -MOVEIT_CLASS_FORWARD(Interface) -MOVEIT_CLASS_FORWARD(Stage) +MOVEIT_CLASS_FORWARD(Interface); +MOVEIT_CLASS_FORWARD(Stage); class InterfaceState; using InterfaceStatePair = std::pair; diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index 447c088a..f2d3e5be 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -43,8 +43,8 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotState) -MOVEIT_CLASS_FORWARD(JointModelGroup) +MOVEIT_CLASS_FORWARD(RobotState); +MOVEIT_CLASS_FORWARD(JointModelGroup); } // namespace core } // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index f2913c3f..bb64dbb7 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -45,7 +45,7 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotState) +MOVEIT_CLASS_FORWARD(RobotState); } } // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index bcad42b1..80ec9d4a 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -46,7 +46,7 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(JointModelGroup) +MOVEIT_CLASS_FORWARD(JointModelGroup); } } // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index 34d9f6ec..dca07f66 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -44,7 +44,7 @@ namespace moveit { namespace task_constructor { namespace solvers { -MOVEIT_CLASS_FORWARD(CartesianPath) +MOVEIT_CLASS_FORWARD(CartesianPath); } namespace stages { diff --git a/core/include/moveit/task_constructor/stages/predicate_filter.h b/core/include/moveit/task_constructor/stages/predicate_filter.h index c8784449..412d6659 100644 --- a/core/include/moveit/task_constructor/stages/predicate_filter.h +++ b/core/include/moveit/task_constructor/stages/predicate_filter.h @@ -39,7 +39,7 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotState) +MOVEIT_CLASS_FORWARD(RobotState); } } // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index df81d2fe..30e7a52b 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -43,7 +43,7 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotModel) +MOVEIT_CLASS_FORWARD(RobotModel); } } // namespace moveit namespace moveit { diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index c2d60727..b825d43c 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -52,21 +52,21 @@ #include namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene) +MOVEIT_CLASS_FORWARD(PlanningScene); } namespace robot_trajectory { -MOVEIT_CLASS_FORWARD(RobotTrajectory) +MOVEIT_CLASS_FORWARD(RobotTrajectory); } namespace moveit { namespace task_constructor { class SolutionBase; -MOVEIT_CLASS_FORWARD(InterfaceState) -MOVEIT_CLASS_FORWARD(Interface) -MOVEIT_CLASS_FORWARD(Stage) -MOVEIT_CLASS_FORWARD(Introspection) +MOVEIT_CLASS_FORWARD(InterfaceState); +MOVEIT_CLASS_FORWARD(Interface); +MOVEIT_CLASS_FORWARD(Stage); +MOVEIT_CLASS_FORWARD(Introspection); /** InterfaceState describes a potential start or goal state for a planning stage. * @@ -296,7 +296,7 @@ private: const InterfaceState* start_ = nullptr; const InterfaceState* end_ = nullptr; }; -MOVEIT_CLASS_FORWARD(SolutionBase) +MOVEIT_CLASS_FORWARD(SolutionBase); /// SubTrajectory connects interface states of ComputeStages class SubTrajectory : public SolutionBase @@ -318,7 +318,7 @@ private: // actual trajectory, might be empty robot_trajectory::RobotTrajectoryConstPtr trajectory_; }; -MOVEIT_CLASS_FORWARD(SubTrajectory) +MOVEIT_CLASS_FORWARD(SubTrajectory); /** Sequence of individual sub solutions * @@ -350,7 +350,7 @@ private: /// series of sub solutions container_type subsolutions_; }; -MOVEIT_CLASS_FORWARD(SolutionSequence) +MOVEIT_CLASS_FORWARD(SolutionSequence); /** Wrap an existing solution * diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index eb2b8e16..6bac28d7 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -50,17 +50,17 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotModel) -MOVEIT_CLASS_FORWARD(RobotState) +MOVEIT_CLASS_FORWARD(RobotModel); +MOVEIT_CLASS_FORWARD(RobotState); } // namespace core } // namespace moveit namespace moveit { namespace task_constructor { -MOVEIT_CLASS_FORWARD(Stage) -MOVEIT_CLASS_FORWARD(ContainerBase) -MOVEIT_CLASS_FORWARD(Task) +MOVEIT_CLASS_FORWARD(Stage); +MOVEIT_CLASS_FORWARD(ContainerBase); +MOVEIT_CLASS_FORWARD(Task); class TaskPrivate; /** A Task is the root of a tree of stages. diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index 68cda9a3..327f1631 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -42,7 +42,7 @@ #include namespace robot_model_loader { -MOVEIT_CLASS_FORWARD(RobotModelLoader) +MOVEIT_CLASS_FORWARD(RobotModelLoader); } namespace moveit { diff --git a/core/test/models.h b/core/test/models.h index a33e3377..11cfcdba 100644 --- a/core/test/models.h +++ b/core/test/models.h @@ -4,7 +4,7 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotModel) +MOVEIT_CLASS_FORWARD(RobotModel); } } // namespace moveit diff --git a/visualization/motion_planning_tasks/src/meta_task_list_model.h b/visualization/motion_planning_tasks/src/meta_task_list_model.h index 09428f7a..2c87b847 100644 --- a/visualization/motion_planning_tasks/src/meta_task_list_model.h +++ b/visualization/motion_planning_tasks/src/meta_task_list_model.h @@ -42,9 +42,9 @@ namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(BaseTaskModel) -MOVEIT_CLASS_FORWARD(TaskListModel) -MOVEIT_CLASS_FORWARD(TaskDisplay) +MOVEIT_CLASS_FORWARD(BaseTaskModel); +MOVEIT_CLASS_FORWARD(TaskListModel); +MOVEIT_CLASS_FORWARD(TaskDisplay); /** MetaTaskListModel maintains a model of multiple registered TaskListModels, * which are grouped in a hierarchical fashion according to the name of the diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index 32839720..ec38dd8c 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -57,16 +57,16 @@ class RosTopicProperty; namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotModel) +MOVEIT_CLASS_FORWARD(RobotModel); } } // namespace moveit namespace rdf_loader { -MOVEIT_CLASS_FORWARD(RDFLoader) +MOVEIT_CLASS_FORWARD(RDFLoader); } namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(DisplaySolution) +MOVEIT_CLASS_FORWARD(DisplaySolution); class TaskListModel; class TaskDisplay : public rviz::Display diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 9ccfeaf9..0386543d 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -60,8 +60,8 @@ class DisplayContext; namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(DisplaySolution) -MOVEIT_CLASS_FORWARD(RemoteTaskModel) +MOVEIT_CLASS_FORWARD(DisplaySolution); +MOVEIT_CLASS_FORWARD(RemoteTaskModel); using StageFactory = PluginlibFactory; using StageFactoryPtr = std::shared_ptr; diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index 5efbde63..f87d2d05 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -54,8 +54,8 @@ class EnumProperty; namespace moveit_rviz_plugin { class TaskSolutionVisualization; -MOVEIT_CLASS_FORWARD(TaskListModel) -MOVEIT_CLASS_FORWARD(TaskPanel) +MOVEIT_CLASS_FORWARD(TaskListModel); +MOVEIT_CLASS_FORWARD(TaskPanel); /// Base class for all sub panels within the Task Panel class SubPanel : public QWidget diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 20882bf1..408b3022 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -41,14 +41,14 @@ namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotState) +MOVEIT_CLASS_FORWARD(RobotState); } } // namespace moveit namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene) +MOVEIT_CLASS_FORWARD(PlanningScene); } namespace robot_trajectory { -MOVEIT_CLASS_FORWARD(RobotTrajectory) +MOVEIT_CLASS_FORWARD(RobotTrajectory); } namespace Ogre { class SceneNode; @@ -59,8 +59,8 @@ class DisplayContext; namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(DisplaySolution) -MOVEIT_CLASS_FORWARD(MarkerVisualization) +MOVEIT_CLASS_FORWARD(DisplaySolution); +MOVEIT_CLASS_FORWARD(MarkerVisualization); /** Class representing a task solution for display */ class DisplaySolution diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h index 09a7cdc4..fe743bf0 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h @@ -17,17 +17,17 @@ class MarkerBase; } // namespace rviz namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene) +MOVEIT_CLASS_FORWARD(PlanningScene); } namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotState) +MOVEIT_CLASS_FORWARD(RobotState); } } // namespace moveit namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(MarkerVisualization) +MOVEIT_CLASS_FORWARD(MarkerVisualization); /** Container for all markers created from a vector of Marker messages * diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index ee53493a..d78dab5f 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -65,22 +65,22 @@ class PanelDockWidget; namespace moveit { namespace core { -MOVEIT_CLASS_FORWARD(RobotModel) +MOVEIT_CLASS_FORWARD(RobotModel); } } // namespace moveit namespace planning_scene { -MOVEIT_CLASS_FORWARD(PlanningScene) +MOVEIT_CLASS_FORWARD(PlanningScene); } namespace robot_trajectory { -MOVEIT_CLASS_FORWARD(RobotTrajectory) +MOVEIT_CLASS_FORWARD(RobotTrajectory); } namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(RobotStateVisualization) -MOVEIT_CLASS_FORWARD(TaskSolutionVisualization) -MOVEIT_CLASS_FORWARD(PlanningSceneRender) -MOVEIT_CLASS_FORWARD(DisplaySolution) +MOVEIT_CLASS_FORWARD(RobotStateVisualization); +MOVEIT_CLASS_FORWARD(TaskSolutionVisualization); +MOVEIT_CLASS_FORWARD(PlanningSceneRender); +MOVEIT_CLASS_FORWARD(DisplaySolution); class TaskSolutionPanel; class MarkerVisualizationProperty; From 924e733a42c92597fef48bc9f9405fb8c53a7c49 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 3 Jun 2021 10:54:34 +0200 Subject: [PATCH 10/17] use clang for clang-tidy job And add warnings for gcc AND clang builds (as they can differ slightly). Clang does not support -Wused-but-set-parameter yet, though support is [on the way](https://reviews.llvm.org/D100581). So this gets rid of this warning we see in CI: > error: unknown warning option '-Wno-unused-but-set-parameter'; did you mean '-Wno-unused-parameter'? --- .github/workflows/ci.yaml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index bda2f42c..22a6f879 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -18,13 +18,15 @@ jobs: NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: master-source - NAME: clang - CXX: clang++ - - 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 -Wno-unused-but-set-parameter + - IMAGE: noetic-source + CXX: clang++ + CLANG_TIDY: true + CXXFLAGS: >- + -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" From 9d53987e534236251fa289cf42560bff1e174169 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 3 Jun 2021 11:04:29 +0200 Subject: [PATCH 11/17] satisfy clang-tidy & -Werror -Wall -Wextra --- .../moveit/task_constructor/cost_terms.h | 2 +- core/test/test_serial.cpp | 20 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index e9d8d502..6e3e8de5 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -118,7 +118,7 @@ class PathLength : public TrajectoryCostTerm public: PathLength() = default; PathLength(std::vector j) : joints{ std::move(j) } {}; - double operator()(const SubTrajectory&, std::string&) const override; + double operator()(const SubTrajectory& s, std::string& comment) const override; std::vector joints; }; diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index a3dd4863..424292c1 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -30,9 +30,9 @@ struct PredefinedCosts : CostTerm return cost_; } - double operator()(const SubTrajectory& s, std::string& comment) const override { return cost(); } - double operator()(const SolutionSequence& s, std::string& comment) const override { return cost(); } - double operator()(const WrappedSolution& s, std::string& comment) const override { return cost(); } + double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override { return cost(); } + double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override { return cost(); } + double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override { return cost(); } }; /** Generator creating solutions with given costs */ @@ -197,9 +197,9 @@ TEST_F(Pruning, PropagatorFailure) { add(task, new ForwardMockup({ INF })); EXPECT_FALSE(task.plan()); - ASSERT_EQ(task.solutions().size(), 0); + ASSERT_EQ(task.solutions().size(), 0u); // ForwardMockup fails, so the backward stage should never compute - EXPECT_EQ(back->calls_, 0); + EXPECT_EQ(back->calls_, 0u); } TEST_F(Pruning, PruningMultiForward) { @@ -215,7 +215,7 @@ TEST_F(Pruning, PruningMultiForward) { // the second (infeasible) solution in the last stage must not disable // the earlier partial solution just because they share stage solutions - ASSERT_EQ(task.solutions().size(), 1); + ASSERT_EQ(task.solutions().size(), 1u); EXPECT_EQ((*task.solutions().begin())->cost(), 0u); } @@ -273,7 +273,7 @@ TEST_F(Pruning, PropagateIntoContainer) { // the failure in the backward stage (outside the container) // should prune the expected computation of con inside the container - EXPECT_EQ(con->calls_, 0); + EXPECT_EQ(con->calls_, 0u); } TEST_F(Pruning, PropagateFromContainerPull) { @@ -288,7 +288,7 @@ TEST_F(Pruning, PropagateFromContainerPull) { EXPECT_FALSE(task.plan()); // the failure inside the container should prune computing of back - EXPECT_EQ(back->calls_, 0); + EXPECT_EQ(back->calls_, 0u); } TEST_F(Pruning, PropagateFromContainerPush) { @@ -302,7 +302,7 @@ TEST_F(Pruning, PropagateFromContainerPush) { EXPECT_FALSE(task.plan()); // the failure inside container should prune computing of con - EXPECT_EQ(con->calls_, 0); + EXPECT_EQ(con->calls_, 0u); } TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) { @@ -318,5 +318,5 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) { EXPECT_TRUE(task.plan()); // the failure in one branch of Alternatives must not prune computing back - EXPECT_EQ(back->calls_, 1); + EXPECT_EQ(back->calls_, 1u); } From 1f51124ae5f6864d24ac02b8adfb8ee7f6c1f44f Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 3 Jun 2021 11:18:10 +0200 Subject: [PATCH 12/17] add missing virtual destructor to CostTerm Correctly pointed out by the clang-tidy CI. --- core/include/moveit/task_constructor/cost_terms.h | 1 + 1 file changed, 1 insertion(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 6e3e8de5..dbbc368e 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -56,6 +56,7 @@ class CostTerm public: CostTerm() = default; CostTerm(std::nullptr_t) : CostTerm{} {} + virtual ~CostTerm() = default; virtual double operator()(const SubTrajectory& s, std::string& comment) const; virtual double operator()(const SolutionSequence& s, std::string& comment) const; From 0e40b464ec262b14a44245957c1d7017e36c8e86 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 3 Jun 2021 11:45:47 +0200 Subject: [PATCH 13/17] support TYPED_TEST_SUITE Get rid of deprecation warning if new variant exists. --- core/test/test_cost_queue.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index c6b56823..0f4a0433 100644 --- a/core/test/test_cost_queue.cpp +++ b/core/test/test_cost_queue.cpp @@ -62,7 +62,11 @@ protected: }; // set of template types to test for using TypeInstances = ::testing::Types; +#ifdef TYPED_TEST_SUITE +TYPED_TEST_SUITE(ValueOrPointeeLessTest, TypeInstances); +#else TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances); +#endif TYPED_TEST(ValueOrPointeeLessTest, less) { EXPECT_TRUE(this->less(2, 3)); EXPECT_FALSE(this->less(1, 1)); @@ -101,7 +105,11 @@ protected: SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \ this->pushAndValidate(cost, __VA_ARGS__); \ } +#ifdef TYPED_TEST_SUITE +TYPED_TEST_SUITE(OrderedTest, TypeInstances); +#else TYPED_TEST_CASE(OrderedTest, TypeInstances); +#endif TYPED_TEST(OrderedTest, sorting) { pushAndValidate(2, { 2 }); pushAndValidate(1, { 1, 2 }); From f95fcf17b4af19679c1ff0aa87a6b2ddbdca90b8 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 7 Jun 2021 14:34:09 +0200 Subject: [PATCH 14/17] include OGRE headers via SYSTEM --- visualization/CMakeLists.txt | 11 +++++++++++ .../motion_planning_tasks/src/CMakeLists.txt | 3 +++ visualization/visualization_tools/CMakeLists.txt | 3 +++ 3 files changed, 17 insertions(+) diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 5d23d982..5065bc9f 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -10,6 +10,17 @@ find_package(catkin REQUIRED COMPONENTS rviz ) +# 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) +foreach(header IN ITEMS OgreRoot.h OgreOverlay.h) + find_path(include_dir ${header} + HINTS ${catkin_INCLUDE_DIRS} + NO_DEFAULT_PATH) + list(REMOVE_ITEM catkin_INCLUDE_DIRS "${include_dir}") + list(APPEND rviz_OGRE_INCLUDE_DIRS "${include_dir}") +endforeach() + # definition needed for boost/math/constants/constants.hpp included by Ogre to compile add_definitions(-DBOOST_MATH_DISABLE_FLOAT128) diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index f2eaa354..832134c7 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -39,6 +39,9 @@ target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ PUBLIC ${catkin_INCLUDE_DIRS} ) +target_include_directories(${MOVEIT_LIB_NAME} SYSTEM + PUBLIC ${rviz_OGRE_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 7ee5bd16..49116a1e 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -35,6 +35,9 @@ target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include PRIVATE ${catkin_INCLUDE_DIRS} ) +target_include_directories(${MOVEIT_LIB_NAME} SYSTEM + PUBLIC ${rviz_OGRE_INCLUDE_DIRS} +) add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) From 5dacc85ff98b48a76586924eed76e8300f55a9ef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 10 Jun 2021 00:40:47 +0200 Subject: [PATCH 15/17] Fix warning: control reaches end of non-void function --- .../motion_planning_tasks/properties/property_from_yaml.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp index 88cb4459..a46d18bf 100644 --- a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp +++ b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp @@ -151,9 +151,8 @@ rviz::Property* Parser::process(const yaml_event_t& event, const QString& name, case YAML_SCALAR_EVENT: return createScalar(name, description, byteArray(event), old); default: - break; + throw std::runtime_error("Unhandled YAML event"); } - assert(false); // should not be reached } // Try to set numeric or arbitrary scalar value from YAML node. Needs to match old's type. From 3457c91ff23bce0af8c2af5d0958b9ed5c599fb0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 7 Jun 2021 21:56:08 +0200 Subject: [PATCH 16/17] GHA: lsan.suppressions --- .github/workflows/ci.yaml | 5 ++++- .github/workflows/lsan.suppressions | 1 + 2 files changed, 5 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/lsan.suppressions diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 22a6f879..a2c72c32 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -29,8 +29,11 @@ jobs: -Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy - IMAGE: noetic-source NAME: asan - DOCKER_RUN_OPTS: "-e PRELOAD=libasan.so.5" + 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" + env: CATKIN_LINT: true UNDERLAY: /root/ws_moveit/install diff --git a/.github/workflows/lsan.suppressions b/.github/workflows/lsan.suppressions new file mode 100644 index 00000000..79bf7e06 --- /dev/null +++ b/.github/workflows/lsan.suppressions @@ -0,0 +1 @@ +leak:class_loader From 4aeab27ba3bed47453ffab0b454eb6eb09b54e4f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 11 Jun 2021 09:44:45 +0200 Subject: [PATCH 17/17] Fix placement of Panel widget Since #1611 and 41d62f5749b05c68131b752c605191518eaf19d5, the floating argument for panels is correctly handled. Here, we don't want the panel to be floating. --- visualization/motion_planning_tasks/src/task_panel.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 7b37a602..b73e2e15 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -155,9 +155,8 @@ void TaskPanel::request(rviz::WindowManagerInterface* window_manager) { if (SINGLETON || !vis_frame) return; // already defined, nothing to do - QDockWidget* dock = - vis_frame->addPanelByName("Motion Planning Tasks", "moveit_task_constructor/Motion Planning Tasks", - Qt::LeftDockWidgetArea, true /* floating */); + QDockWidget* dock = vis_frame->addPanelByName( + "Motion Planning Tasks", "moveit_task_constructor/Motion Planning Tasks", Qt::LeftDockWidgetArea); Q_UNUSED(dock); assert(dock->widget() == SINGLETON); }