Merge branch 'master' into wip-python-api

.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

.github/workflows/lsan.suppressions

@@ -0,0 +1 @@
+leak:class_loader

core/CMakeLists.txt

@@ -21,6 +21,7 @@ catkin_package(
 	LIBRARIES
 		${PROJECT_NAME}
 		${PROJECT_NAME}_stages
+		${PROJECT_NAME}_stage_plugins
 	INCLUDE_DIRS
 		include
 	CATKIN_DEPENDS

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
 

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 @@ class Constant : public CostTerm
 /// trajectory length (interpolated between waypoints)
 class PathLength : public TrajectoryCostTerm
 {
-	// TODO(v4hn): allow to consider specific joints only
 public:
+	PathLength() = default;
+	PathLength(std::vector<std::string> j) : joints{ std::move(j) } {};
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
+
+	std::vector<std::string> joints;
 };
 
 /// execution duration of the whole trajectory

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;

core/include/moveit/task_constructor/marker_tools.h

@@ -6,12 +6,12 @@
 #include <functional>
 
 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
 

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

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
  *

core/include/moveit/task_constructor/solvers/pipeline_planner.h

@@ -42,14 +42,14 @@
 #include <moveit/macros/class_forward.h>
 
 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

core/include/moveit/task_constructor/solvers/planner_interface.h

@@ -44,24 +44,24 @@
 #include <Eigen/Geometry>
 
 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
 
 namespace moveit {
 namespace task_constructor {
 namespace solvers {
 
-MOVEIT_CLASS_FORWARD(PlannerInterface)
+MOVEIT_CLASS_FORWARD(PlannerInterface);
 class PlannerInterface
 {
 	// these properties take precedence over stage properties

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<const InterfaceState&, const InterfaceState&>;
 

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
 

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
 

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
 

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 {

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
 

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 {

core/include/moveit/task_constructor/storage.h

@@ -52,21 +52,21 @@
 #include <functional>
 
 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 @@ class SolutionBase
 	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 @@ class SubTrajectory : public SolutionBase
 	// 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 @@ class SolutionSequence : public SolutionBase
 	/// series of sub solutions
 	container_type subsolutions_;
 };
-MOVEIT_CLASS_FORWARD(SolutionSequence)
+MOVEIT_CLASS_FORWARD(SolutionSequence);
 
 /** Wrap an existing solution
  *

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 @@ class Task : protected WrapperBase
 	void onNewSolution(const SolutionBase& s) override;
 
 private:
+	using WrapperBase::init;
 };
 
 inline std::ostream& operator<<(std::ostream& os, const Task& task) {

core/include/moveit/task_constructor/task_p.h

@@ -42,7 +42,7 @@
 #include <moveit/task_constructor/task.h>
 
 namespace robot_model_loader {
-MOVEIT_CLASS_FORWARD(RobotModelLoader)
+MOVEIT_CLASS_FORWARD(RobotModelLoader);
 }
 
 namespace moveit {

core/motion_planning_stages_plugin_description.xml

@@ -1,4 +1,4 @@
-<library path="libmoveit_task_constructor_core_stages">
+<library path="libmoveit_task_constructor_core_stage_plugins">
   <class name="moveit_task_constructor/Current State"
          type="moveit::task_constructor::stages::CurrentState"
          base_class_type="moveit::task_constructor::Stage">

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)
-
-	# Add rostests
-	add_rostest(test/rostest_mtc.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)
+	endif()
 endif()
 
 roslint_python()

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) {