diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 97639759..5b2cf110 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -18,13 +18,24 @@ jobs: NAME: ccov TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_FLAGS="--coverage" - IMAGE: master-source - NAME: clang - CXX: clang++ + 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 + -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 - 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 @@ -58,7 +69,7 @@ jobs: uses: actions/upload-artifact@v2 if: failure() with: - name: test-results + name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }} path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - name: Collect coverage information uses: rhaschke/lcov-action@master 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 diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 6746d0e3..57a89f5d 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -21,6 +21,7 @@ catkin_package( LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_stages + ${PROJECT_NAME}_stage_plugins INCLUDE_DIRS include CATKIN_DEPENDS 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/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index b8fcb369..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; @@ -115,9 +116,12 @@ public: /// trajectory length (interpolated between waypoints) class PathLength : public TrajectoryCostTerm { - // TODO(v4hn): allow to consider specific joints only public: + PathLength() = default; + PathLength(std::vector j) : joints{ std::move(j) } {}; double operator()(const SubTrajectory& s, std::string& comment) const override; + + std::vector joints; }; /// execution duration of the whole trajectory 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 9823f3a8..6773206b 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 4c4e9eee..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. @@ -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/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/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/python/CMakeLists.txt b/core/python/CMakeLists.txt index 88aa73a2..58f2c7ec 100644 --- a/core/python/CMakeLists.txt +++ b/core/python/CMakeLists.txt @@ -27,10 +27,14 @@ if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) # Add folders to be run by python nosetests - catkin_add_nosetests(test) + if(DEFINED ENV{PRELOAD}) + message(WARNING "Skipping python tests due to asan") + else() + catkin_add_nosetests(test) - # Add rostests - add_rostest(test/rostest_mtc.test) + # Add rostests + add_rostest(test/rostest_mtc.test) + endif() endif() roslint_python() diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index 67768817..b3104265 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -256,7 +256,7 @@ void export_core(pybind11::module& m) { return py::make_iterator(children.begin(), children.end()); }, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives .def("reset", &Task::reset) - .def("init", &Task::init) + .def("init", py::overload_cast<>(&Task::init)) .def("plan", &Task::plan, py::arg("max_solutions") = 0) .def("preempt", &Task::preempt) .def("publish", [](Task& self, const SolutionBasePtr& solution) { diff --git a/core/python/pybind11 b/core/python/pybind11 index 3c92b9fb..8f6ca713 160000 --- a/core/python/pybind11 +++ b/core/python/pybind11 @@ -1 +1 @@ -Subproject commit 3c92b9fbe6b05c254e7d20a7b9d8040c92029290 +Subproject commit 8f6ca71390f4cc1faf8e060146f347fa387ecd6a diff --git a/core/python/test/rostest_mtc.test b/core/python/test/rostest_mtc.test index 34cffc58..b028af75 100644 --- a/core/python/test/rostest_mtc.test +++ b/core/python/test/rostest_mtc.test @@ -1,5 +1,5 @@ - - + + 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; } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 967ab1a0..2c0cd894 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 PUBLIC ${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) 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/core/test/test_cost_queue.cpp b/core/test/test_cost_queue.cpp index 86888c04..0f4a0433 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) { @@ -59,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)); @@ -98,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 }); 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(); 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); } 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..89a500bd --- /dev/null +++ b/demo/test/pick_place.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + 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(); +} 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/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. 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/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.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 885543cc..b73e2e15 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -155,9 +155,9 @@ 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); } 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/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}) 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;