mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Merge branch 'master' into wip-python-api
This commit is contained in:
commit
ced362f5f6
19
.github/workflows/ci.yaml
vendored
19
.github/workflows/ci.yaml
vendored
@ -18,13 +18,24 @@ jobs:
|
|||||||
NAME: ccov
|
NAME: ccov
|
||||||
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_FLAGS="--coverage"
|
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_FLAGS="--coverage"
|
||||||
- IMAGE: master-source
|
- IMAGE: master-source
|
||||||
NAME: clang
|
CXXFLAGS: >-
|
||||||
CXX: clang++
|
-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
|
- IMAGE: noetic-source
|
||||||
|
CXX: clang++
|
||||||
CLANG_TIDY: true
|
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:
|
env:
|
||||||
CATKIN_LINT: true
|
CATKIN_LINT: true
|
||||||
CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy"
|
|
||||||
UNDERLAY: /root/ws_moveit/install
|
UNDERLAY: /root/ws_moveit/install
|
||||||
DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
|
DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
|
||||||
CCACHE_DIR: ${{ github.workspace }}/.ccache
|
CCACHE_DIR: ${{ github.workspace }}/.ccache
|
||||||
@ -58,7 +69,7 @@ jobs:
|
|||||||
uses: actions/upload-artifact@v2
|
uses: actions/upload-artifact@v2
|
||||||
if: failure()
|
if: failure()
|
||||||
with:
|
with:
|
||||||
name: test-results
|
name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }}
|
||||||
path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
|
path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
|
||||||
- name: Collect coverage information
|
- name: Collect coverage information
|
||||||
uses: rhaschke/lcov-action@master
|
uses: rhaschke/lcov-action@master
|
||||||
|
1
.github/workflows/lsan.suppressions
vendored
Normal file
1
.github/workflows/lsan.suppressions
vendored
Normal file
@ -0,0 +1 @@
|
|||||||
|
leak:class_loader
|
@ -21,6 +21,7 @@ catkin_package(
|
|||||||
LIBRARIES
|
LIBRARIES
|
||||||
${PROJECT_NAME}
|
${PROJECT_NAME}
|
||||||
${PROJECT_NAME}_stages
|
${PROJECT_NAME}_stages
|
||||||
|
${PROJECT_NAME}_stage_plugins
|
||||||
INCLUDE_DIRS
|
INCLUDE_DIRS
|
||||||
include
|
include
|
||||||
CATKIN_DEPENDS
|
CATKIN_DEPENDS
|
||||||
|
@ -51,8 +51,8 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(JointModelGroup)
|
MOVEIT_CLASS_FORWARD(JointModelGroup);
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
} // namespace core
|
} // namespace core
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
|
@ -56,6 +56,7 @@ class CostTerm
|
|||||||
public:
|
public:
|
||||||
CostTerm() = default;
|
CostTerm() = default;
|
||||||
CostTerm(std::nullptr_t) : CostTerm{} {}
|
CostTerm(std::nullptr_t) : CostTerm{} {}
|
||||||
|
virtual ~CostTerm() = default;
|
||||||
|
|
||||||
virtual double operator()(const SubTrajectory& s, std::string& comment) const;
|
virtual double operator()(const SubTrajectory& s, std::string& comment) const;
|
||||||
virtual double operator()(const SolutionSequence& 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)
|
/// trajectory length (interpolated between waypoints)
|
||||||
class PathLength : public TrajectoryCostTerm
|
class PathLength : public TrajectoryCostTerm
|
||||||
{
|
{
|
||||||
// TODO(v4hn): allow to consider specific joints only
|
|
||||||
public:
|
public:
|
||||||
|
PathLength() = default;
|
||||||
|
PathLength(std::vector<std::string> j) : joints{ std::move(j) } {};
|
||||||
double operator()(const SubTrajectory& s, std::string& comment) const override;
|
double operator()(const SubTrajectory& s, std::string& comment) const override;
|
||||||
|
|
||||||
|
std::vector<std::string> joints;
|
||||||
};
|
};
|
||||||
|
|
||||||
/// execution duration of the whole trajectory
|
/// execution duration of the whole trajectory
|
||||||
|
@ -52,8 +52,8 @@
|
|||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Stage)
|
MOVEIT_CLASS_FORWARD(Stage);
|
||||||
MOVEIT_CLASS_FORWARD(SolutionBase)
|
MOVEIT_CLASS_FORWARD(SolutionBase);
|
||||||
|
|
||||||
class TaskPrivate;
|
class TaskPrivate;
|
||||||
class IntrospectionPrivate;
|
class IntrospectionPrivate;
|
||||||
|
@ -6,12 +6,12 @@
|
|||||||
#include <functional>
|
#include <functional>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
}
|
}
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
MOVEIT_CLASS_FORWARD(LinkModel)
|
MOVEIT_CLASS_FORWARD(LinkModel);
|
||||||
} // namespace core
|
} // namespace core
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
|
@ -44,7 +44,7 @@ namespace moveit {
|
|||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
namespace solvers {
|
namespace solvers {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(CartesianPath)
|
MOVEIT_CLASS_FORWARD(CartesianPath);
|
||||||
|
|
||||||
/** Use MoveIt's computeCartesianPath() to generate a straigh-line path between to scenes */
|
/** Use MoveIt's computeCartesianPath() to generate a straigh-line path between to scenes */
|
||||||
class CartesianPath : public PlannerInterface
|
class CartesianPath : public PlannerInterface
|
||||||
|
@ -45,7 +45,7 @@ namespace moveit {
|
|||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
namespace solvers {
|
namespace solvers {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(JointInterpolationPlanner)
|
MOVEIT_CLASS_FORWARD(JointInterpolationPlanner);
|
||||||
|
|
||||||
/** Interpolate a trajectory between states in joint space
|
/** Interpolate a trajectory between states in joint space
|
||||||
*
|
*
|
||||||
|
@ -42,14 +42,14 @@
|
|||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
namespace planning_pipeline {
|
namespace planning_pipeline {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningPipeline)
|
MOVEIT_CLASS_FORWARD(PlanningPipeline);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
namespace solvers {
|
namespace solvers {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(PipelinePlanner)
|
MOVEIT_CLASS_FORWARD(PipelinePlanner);
|
||||||
|
|
||||||
/** Use MoveIt's PlanningPipeline to plan a trajectory between to scenes */
|
/** Use MoveIt's PlanningPipeline to plan a trajectory between to scenes */
|
||||||
class PipelinePlanner : public PlannerInterface
|
class PipelinePlanner : public PlannerInterface
|
||||||
|
@ -44,16 +44,16 @@
|
|||||||
#include <Eigen/Geometry>
|
#include <Eigen/Geometry>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
}
|
}
|
||||||
namespace robot_trajectory {
|
namespace robot_trajectory {
|
||||||
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
MOVEIT_CLASS_FORWARD(RobotTrajectory);
|
||||||
}
|
}
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(LinkModel)
|
MOVEIT_CLASS_FORWARD(LinkModel);
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||||
MOVEIT_CLASS_FORWARD(JointModelGroup)
|
MOVEIT_CLASS_FORWARD(JointModelGroup);
|
||||||
} // namespace core
|
} // namespace core
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
@ -61,7 +61,7 @@ namespace moveit {
|
|||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
namespace solvers {
|
namespace solvers {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(PlannerInterface)
|
MOVEIT_CLASS_FORWARD(PlannerInterface);
|
||||||
class PlannerInterface
|
class PlannerInterface
|
||||||
{
|
{
|
||||||
// these properties take precedence over stage properties
|
// these properties take precedence over stage properties
|
||||||
|
@ -52,20 +52,20 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace planning_pipeline {
|
namespace planning_pipeline {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningPipeline)
|
MOVEIT_CLASS_FORWARD(PlanningPipeline);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace robot_trajectory {
|
namespace robot_trajectory {
|
||||||
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
MOVEIT_CLASS_FORWARD(RobotTrajectory);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
@ -108,8 +108,8 @@ constexpr InterfaceFlags UNKNOWN;
|
|||||||
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
|
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
|
||||||
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
|
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Interface)
|
MOVEIT_CLASS_FORWARD(Interface);
|
||||||
MOVEIT_CLASS_FORWARD(Stage)
|
MOVEIT_CLASS_FORWARD(Stage);
|
||||||
class InterfaceState;
|
class InterfaceState;
|
||||||
using InterfaceStatePair = std::pair<const InterfaceState&, const InterfaceState&>;
|
using InterfaceStatePair = std::pair<const InterfaceState&, const InterfaceState&>;
|
||||||
|
|
||||||
|
@ -43,8 +43,8 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
MOVEIT_CLASS_FORWARD(JointModelGroup)
|
MOVEIT_CLASS_FORWARD(JointModelGroup);
|
||||||
} // namespace core
|
} // namespace core
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
|
@ -45,7 +45,7 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
|
@ -46,7 +46,7 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(JointModelGroup)
|
MOVEIT_CLASS_FORWARD(JointModelGroup);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
|
@ -44,7 +44,7 @@ namespace moveit {
|
|||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
namespace solvers {
|
namespace solvers {
|
||||||
MOVEIT_CLASS_FORWARD(CartesianPath)
|
MOVEIT_CLASS_FORWARD(CartesianPath);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace stages {
|
namespace stages {
|
||||||
|
@ -39,7 +39,7 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
|
@ -43,7 +43,7 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
|
@ -52,21 +52,21 @@
|
|||||||
#include <functional>
|
#include <functional>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace robot_trajectory {
|
namespace robot_trajectory {
|
||||||
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
MOVEIT_CLASS_FORWARD(RobotTrajectory);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
class SolutionBase;
|
class SolutionBase;
|
||||||
MOVEIT_CLASS_FORWARD(InterfaceState)
|
MOVEIT_CLASS_FORWARD(InterfaceState);
|
||||||
MOVEIT_CLASS_FORWARD(Interface)
|
MOVEIT_CLASS_FORWARD(Interface);
|
||||||
MOVEIT_CLASS_FORWARD(Stage)
|
MOVEIT_CLASS_FORWARD(Stage);
|
||||||
MOVEIT_CLASS_FORWARD(Introspection)
|
MOVEIT_CLASS_FORWARD(Introspection);
|
||||||
|
|
||||||
/** InterfaceState describes a potential start or goal state for a planning stage.
|
/** InterfaceState describes a potential start or goal state for a planning stage.
|
||||||
*
|
*
|
||||||
@ -296,7 +296,7 @@ private:
|
|||||||
const InterfaceState* start_ = nullptr;
|
const InterfaceState* start_ = nullptr;
|
||||||
const InterfaceState* end_ = nullptr;
|
const InterfaceState* end_ = nullptr;
|
||||||
};
|
};
|
||||||
MOVEIT_CLASS_FORWARD(SolutionBase)
|
MOVEIT_CLASS_FORWARD(SolutionBase);
|
||||||
|
|
||||||
/// SubTrajectory connects interface states of ComputeStages
|
/// SubTrajectory connects interface states of ComputeStages
|
||||||
class SubTrajectory : public SolutionBase
|
class SubTrajectory : public SolutionBase
|
||||||
@ -318,7 +318,7 @@ private:
|
|||||||
// actual trajectory, might be empty
|
// actual trajectory, might be empty
|
||||||
robot_trajectory::RobotTrajectoryConstPtr trajectory_;
|
robot_trajectory::RobotTrajectoryConstPtr trajectory_;
|
||||||
};
|
};
|
||||||
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
MOVEIT_CLASS_FORWARD(SubTrajectory);
|
||||||
|
|
||||||
/** Sequence of individual sub solutions
|
/** Sequence of individual sub solutions
|
||||||
*
|
*
|
||||||
@ -350,7 +350,7 @@ private:
|
|||||||
/// series of sub solutions
|
/// series of sub solutions
|
||||||
container_type subsolutions_;
|
container_type subsolutions_;
|
||||||
};
|
};
|
||||||
MOVEIT_CLASS_FORWARD(SolutionSequence)
|
MOVEIT_CLASS_FORWARD(SolutionSequence);
|
||||||
|
|
||||||
/** Wrap an existing solution
|
/** Wrap an existing solution
|
||||||
*
|
*
|
||||||
|
@ -50,17 +50,17 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
} // namespace core
|
} // namespace core
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Stage)
|
MOVEIT_CLASS_FORWARD(Stage);
|
||||||
MOVEIT_CLASS_FORWARD(ContainerBase)
|
MOVEIT_CLASS_FORWARD(ContainerBase);
|
||||||
MOVEIT_CLASS_FORWARD(Task)
|
MOVEIT_CLASS_FORWARD(Task);
|
||||||
|
|
||||||
class TaskPrivate;
|
class TaskPrivate;
|
||||||
/** A Task is the root of a tree of stages.
|
/** A Task is the root of a tree of stages.
|
||||||
@ -151,6 +151,7 @@ protected:
|
|||||||
void onNewSolution(const SolutionBase& s) override;
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
using WrapperBase::init;
|
||||||
};
|
};
|
||||||
|
|
||||||
inline std::ostream& operator<<(std::ostream& os, const Task& task) {
|
inline std::ostream& operator<<(std::ostream& os, const Task& task) {
|
||||||
|
@ -42,7 +42,7 @@
|
|||||||
#include <moveit/task_constructor/task.h>
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
namespace robot_model_loader {
|
namespace robot_model_loader {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
MOVEIT_CLASS_FORWARD(RobotModelLoader);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
|
@ -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"
|
<class name="moveit_task_constructor/Current State"
|
||||||
type="moveit::task_constructor::stages::CurrentState"
|
type="moveit::task_constructor::stages::CurrentState"
|
||||||
base_class_type="moveit::task_constructor::Stage">
|
base_class_type="moveit::task_constructor::Stage">
|
||||||
|
@ -27,10 +27,14 @@ if(CATKIN_ENABLE_TESTING)
|
|||||||
find_package(rostest REQUIRED)
|
find_package(rostest REQUIRED)
|
||||||
|
|
||||||
# Add folders to be run by python nosetests
|
# 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 rostests
|
||||||
add_rostest(test/rostest_mtc.test)
|
add_rostest(test/rostest_mtc.test)
|
||||||
|
endif()
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
roslint_python()
|
roslint_python()
|
||||||
|
@ -256,7 +256,7 @@ void export_core(pybind11::module& m) {
|
|||||||
return py::make_iterator(children.begin(), children.end());
|
return py::make_iterator(children.begin(), children.end());
|
||||||
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
|
}, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
|
||||||
.def("reset", &Task::reset)
|
.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("plan", &Task::plan, py::arg("max_solutions") = 0)
|
||||||
.def("preempt", &Task::preempt)
|
.def("preempt", &Task::preempt)
|
||||||
.def("publish", [](Task& self, const SolutionBasePtr& solution) {
|
.def("publish", [](Task& self, const SolutionBasePtr& solution) {
|
||||||
|
@ -1 +1 @@
|
|||||||
Subproject commit 3c92b9fbe6b05c254e7d20a7b9d8040c92029290
|
Subproject commit 8f6ca71390f4cc1faf8e060146f347fa387ecd6a
|
@ -1,5 +1,5 @@
|
|||||||
<launch>
|
<launch>
|
||||||
<include file="$(find moveit_resources_fanuc_moveit_config)/launch/test_environment.launch" />
|
<include file="$(find moveit_resources_fanuc_moveit_config)/launch/test_environment.launch" />
|
||||||
<test pkg="moveit_task_constructor_core" type="rostest_mtc.py" test-name="rostest_mtc" time-limit="300" args="" />
|
<test pkg="moveit_task_constructor_core" type="rostest_mtc.py" test-name="rostest_mtc" time-limit="60" args="" />
|
||||||
<test pkg="moveit_task_constructor_core" type="rostest_trampoline.py" test-name="rostest_trampoline" time-limit="300" args="" />
|
<test pkg="moveit_task_constructor_core" type="rostest_trampoline.py" test-name="rostest_trampoline" time-limit="60" args="" />
|
||||||
</launch>
|
</launch>
|
||||||
|
@ -110,12 +110,28 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment
|
|||||||
double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
|
double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
|
||||||
const auto& traj = s.trajectory();
|
const auto& traj = s.trajectory();
|
||||||
|
|
||||||
if (traj == nullptr)
|
if (traj == nullptr || traj->getWayPointCount() == 0)
|
||||||
return 0.0;
|
return 0.0;
|
||||||
|
|
||||||
|
std::vector<const robot_model::JointModel*> 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 };
|
double path_length{ 0.0 };
|
||||||
for (size_t i = 1; i < traj->getWayPointCount(); ++i)
|
for (size_t i = 1; i < traj->getWayPointCount(); ++i) {
|
||||||
path_length += traj->getWayPoint(i - 1).distance(traj->getWayPoint(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;
|
return path_length;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -41,6 +41,11 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
)
|
)
|
||||||
target_link_libraries(${PROJECT_NAME}_stages PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES})
|
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}
|
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||||
|
@ -96,7 +96,3 @@ void CurrentState::compute() {
|
|||||||
} // namespace stages
|
} // namespace stages
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
/// register plugin
|
|
||||||
#include <pluginlib/class_list_macros.h>
|
|
||||||
PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::CurrentState, moveit::task_constructor::Stage)
|
|
||||||
|
7
core/src/stages/plugins.cpp
Normal file
7
core/src/stages/plugins.cpp
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
|
|
||||||
|
#include <pluginlib/class_list_macros.h>
|
||||||
|
|
||||||
|
/// register plugins to use with ClassLoader
|
||||||
|
|
||||||
|
PLUGINLIB_EXPORT_CLASS(moveit::task_constructor::stages::CurrentState, moveit::task_constructor::Stage)
|
@ -4,7 +4,7 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
|
@ -16,7 +16,10 @@ int create(int cost) {
|
|||||||
}
|
}
|
||||||
template <>
|
template <>
|
||||||
int* create(int cost) {
|
int* create(int cost) {
|
||||||
return new int(cost);
|
// return new int(cost), but store values for clean memory management
|
||||||
|
static std::list<int> storage;
|
||||||
|
storage.push_back(cost);
|
||||||
|
return &storage.back();
|
||||||
}
|
}
|
||||||
template <>
|
template <>
|
||||||
mtc::SolutionBasePtr create(int cost) {
|
mtc::SolutionBasePtr create(int cost) {
|
||||||
@ -59,7 +62,11 @@ protected:
|
|||||||
};
|
};
|
||||||
// set of template types to test for
|
// set of template types to test for
|
||||||
using TypeInstances = ::testing::Types<int, int*, mtc::SolutionBasePtr, mtc::SolutionBaseConstPtr>;
|
using TypeInstances = ::testing::Types<int, int*, mtc::SolutionBasePtr, mtc::SolutionBaseConstPtr>;
|
||||||
|
#ifdef TYPED_TEST_SUITE
|
||||||
|
TYPED_TEST_SUITE(ValueOrPointeeLessTest, TypeInstances);
|
||||||
|
#else
|
||||||
TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances);
|
TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances);
|
||||||
|
#endif
|
||||||
TYPED_TEST(ValueOrPointeeLessTest, less) {
|
TYPED_TEST(ValueOrPointeeLessTest, less) {
|
||||||
EXPECT_TRUE(this->less(2, 3));
|
EXPECT_TRUE(this->less(2, 3));
|
||||||
EXPECT_FALSE(this->less(1, 1));
|
EXPECT_FALSE(this->less(1, 1));
|
||||||
@ -98,7 +105,11 @@ protected:
|
|||||||
SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \
|
SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \
|
||||||
this->pushAndValidate(cost, __VA_ARGS__); \
|
this->pushAndValidate(cost, __VA_ARGS__); \
|
||||||
}
|
}
|
||||||
|
#ifdef TYPED_TEST_SUITE
|
||||||
|
TYPED_TEST_SUITE(OrderedTest, TypeInstances);
|
||||||
|
#else
|
||||||
TYPED_TEST_CASE(OrderedTest, TypeInstances);
|
TYPED_TEST_CASE(OrderedTest, TypeInstances);
|
||||||
|
#endif
|
||||||
TYPED_TEST(OrderedTest, sorting) {
|
TYPED_TEST(OrderedTest, sorting) {
|
||||||
pushAndValidate(2, { 2 });
|
pushAndValidate(2, { 2 });
|
||||||
pushAndValidate(1, { 1, 2 });
|
pushAndValidate(1, { 1, 2 });
|
||||||
|
@ -112,6 +112,7 @@ public:
|
|||||||
auto impl{ this->pimpl() };
|
auto impl{ this->pimpl() };
|
||||||
this->reset();
|
this->reset();
|
||||||
|
|
||||||
|
dummy->clear();
|
||||||
state_start.reset();
|
state_start.reset();
|
||||||
state_end.reset();
|
state_end.reset();
|
||||||
|
|
||||||
|
@ -30,9 +30,9 @@ struct PredefinedCosts : CostTerm
|
|||||||
return cost_;
|
return cost_;
|
||||||
}
|
}
|
||||||
|
|
||||||
double operator()(const SubTrajectory& 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 SolutionSequence& /*s*/, std::string& /*comment*/) const override { return cost(); }
|
||||||
double operator()(const WrappedSolution& 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 */
|
/** Generator creating solutions with given costs */
|
||||||
@ -197,9 +197,9 @@ TEST_F(Pruning, PropagatorFailure) {
|
|||||||
add(task, new ForwardMockup({ INF }));
|
add(task, new ForwardMockup({ INF }));
|
||||||
|
|
||||||
EXPECT_FALSE(task.plan());
|
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
|
// ForwardMockup fails, so the backward stage should never compute
|
||||||
EXPECT_EQ(back->calls_, 0);
|
EXPECT_EQ(back->calls_, 0u);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Pruning, PruningMultiForward) {
|
TEST_F(Pruning, PruningMultiForward) {
|
||||||
@ -215,7 +215,7 @@ TEST_F(Pruning, PruningMultiForward) {
|
|||||||
|
|
||||||
// the second (infeasible) solution in the last stage must not disable
|
// the second (infeasible) solution in the last stage must not disable
|
||||||
// the earlier partial solution just because they share stage solutions
|
// 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);
|
EXPECT_EQ((*task.solutions().begin())->cost(), 0u);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -273,7 +273,7 @@ TEST_F(Pruning, PropagateIntoContainer) {
|
|||||||
|
|
||||||
// the failure in the backward stage (outside the container)
|
// the failure in the backward stage (outside the container)
|
||||||
// should prune the expected computation of con inside 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) {
|
TEST_F(Pruning, PropagateFromContainerPull) {
|
||||||
@ -288,7 +288,7 @@ TEST_F(Pruning, PropagateFromContainerPull) {
|
|||||||
EXPECT_FALSE(task.plan());
|
EXPECT_FALSE(task.plan());
|
||||||
|
|
||||||
// the failure inside the container should prune computing of back
|
// the failure inside the container should prune computing of back
|
||||||
EXPECT_EQ(back->calls_, 0);
|
EXPECT_EQ(back->calls_, 0u);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Pruning, PropagateFromContainerPush) {
|
TEST_F(Pruning, PropagateFromContainerPush) {
|
||||||
@ -302,7 +302,7 @@ TEST_F(Pruning, PropagateFromContainerPush) {
|
|||||||
EXPECT_FALSE(task.plan());
|
EXPECT_FALSE(task.plan());
|
||||||
|
|
||||||
// the failure inside container should prune computing of con
|
// the failure inside container should prune computing of con
|
||||||
EXPECT_EQ(con->calls_, 0);
|
EXPECT_EQ(con->calls_, 0u);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
|
TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
|
||||||
@ -318,5 +318,5 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
|
|||||||
EXPECT_TRUE(task.plan());
|
EXPECT_TRUE(task.plan());
|
||||||
|
|
||||||
// the failure in one branch of Alternatives must not prune computing back
|
// the failure in one branch of Alternatives must not prune computing back
|
||||||
EXPECT_EQ(back->calls_, 1);
|
EXPECT_EQ(back->calls_, 1u);
|
||||||
}
|
}
|
||||||
|
@ -11,7 +11,9 @@ find_package(catkin REQUIRED COMPONENTS
|
|||||||
rosparam_shortcuts
|
rosparam_shortcuts
|
||||||
)
|
)
|
||||||
|
|
||||||
catkin_package()
|
catkin_package(
|
||||||
|
CATKIN_DEPENDS roscpp
|
||||||
|
)
|
||||||
|
|
||||||
include_directories(
|
include_directories(
|
||||||
include
|
include
|
||||||
@ -24,7 +26,9 @@ target_link_libraries(cartesian ${catkin_LIBRARIES})
|
|||||||
add_executable(modular src/modular.cpp)
|
add_executable(modular src/modular.cpp)
|
||||||
target_link_libraries(modular ${catkin_LIBRARIES})
|
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_OBJECTS:pick_place_task>)
|
||||||
target_link_libraries(pick_place_demo ${catkin_LIBRARIES})
|
target_link_libraries(pick_place_demo ${catkin_LIBRARIES})
|
||||||
|
|
||||||
add_executable(alternative_path_costs src/alternative_path_costs.cpp)
|
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
|
install(DIRECTORY launch config
|
||||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
add_subdirectory(test)
|
||||||
|
@ -58,9 +58,6 @@
|
|||||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||||
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
|
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
|
||||||
|
|
||||||
#include <actionlib/client/simple_action_client.h>
|
|
||||||
#include <actionlib/server/simple_action_server.h>
|
|
||||||
|
|
||||||
#include <eigen_conversions/eigen_msg.h>
|
#include <eigen_conversions/eigen_msg.h>
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
@ -68,22 +65,27 @@
|
|||||||
namespace moveit_task_constructor_demo {
|
namespace moveit_task_constructor_demo {
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
|
// prepare a demo environment from ROS parameters under pnh
|
||||||
|
void setupDemoScene(ros::NodeHandle& pnh);
|
||||||
|
|
||||||
class PickPlaceTask
|
class PickPlaceTask
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh);
|
PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh);
|
||||||
~PickPlaceTask() = default;
|
~PickPlaceTask() = default;
|
||||||
|
|
||||||
void loadParameters();
|
bool init();
|
||||||
|
|
||||||
void init();
|
|
||||||
|
|
||||||
bool plan();
|
bool plan();
|
||||||
|
|
||||||
bool execute();
|
bool execute();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
ros::NodeHandle nh_;
|
void loadParameters();
|
||||||
|
|
||||||
|
static constexpr char LOGNAME[]{ "pick_place_task" };
|
||||||
|
|
||||||
|
ros::NodeHandle pnh_;
|
||||||
|
|
||||||
std::string task_name_;
|
std::string task_name_;
|
||||||
moveit::task_constructor::TaskPtr task_;
|
moveit::task_constructor::TaskPtr task_;
|
||||||
@ -107,9 +109,6 @@ private:
|
|||||||
std::string hand_close_pose_;
|
std::string hand_close_pose_;
|
||||||
std::string arm_home_pose_;
|
std::string arm_home_pose_;
|
||||||
|
|
||||||
// Execution
|
|
||||||
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> execute_;
|
|
||||||
|
|
||||||
// Pick metrics
|
// Pick metrics
|
||||||
Eigen::Isometry3d grasp_frame_transform_;
|
Eigen::Isometry3d grasp_frame_transform_;
|
||||||
double approach_object_min_dist_;
|
double approach_object_min_dist_;
|
||||||
|
@ -11,10 +11,17 @@
|
|||||||
<license>BSD</license>
|
<license>BSD</license>
|
||||||
|
|
||||||
<buildtool_depend>catkin</buildtool_depend>
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
<depend>moveit_task_constructor_core</depend>
|
|
||||||
<depend>moveit_ros_planning_interface</depend>
|
<depend>roscpp</depend>
|
||||||
<depend>moveit_core</depend>
|
|
||||||
<depend>rosparam_shortcuts</depend>
|
<depend>rosparam_shortcuts</depend>
|
||||||
<build_depend>roscpp</build_depend>
|
|
||||||
|
<depend>moveit_core</depend>
|
||||||
|
<depend>moveit_ros_planning_interface</depend>
|
||||||
|
|
||||||
|
<depend>moveit_task_constructor_core</depend>
|
||||||
|
|
||||||
|
<exec_depend>moveit_task_constructor_capabilities</exec_depend>
|
||||||
<exec_depend>moveit_resources_panda_moveit_config</exec_depend>
|
<exec_depend>moveit_resources_panda_moveit_config</exec_depend>
|
||||||
|
|
||||||
|
<test_depend>rostest</test_depend>
|
||||||
</package>
|
</package>
|
||||||
|
@ -40,83 +40,25 @@
|
|||||||
// MTC pick/place demo implementation
|
// MTC pick/place demo implementation
|
||||||
#include <moveit_task_constructor_demo/pick_place_task.h>
|
#include <moveit_task_constructor_demo/pick_place_task.h>
|
||||||
|
|
||||||
#include <geometry_msgs/Pose.h>
|
|
||||||
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
|
||||||
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
|
||||||
#include <tf2_ros/transform_broadcaster.h>
|
|
||||||
|
|
||||||
constexpr char LOGNAME[] = "moveit_task_constructor_demo";
|
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<double> 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<double> 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) {
|
int main(int argc, char** argv) {
|
||||||
ROS_INFO_NAMED(LOGNAME, "Init node");
|
|
||||||
ros::init(argc, argv, "mtc_tutorial");
|
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);
|
ros::AsyncSpinner spinner(1);
|
||||||
spinner.start();
|
spinner.start();
|
||||||
|
|
||||||
// Add table and object to planning scene
|
moveit_task_constructor_demo::setupDemoScene(pnh);
|
||||||
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());
|
|
||||||
|
|
||||||
// Construct and run pick/place task
|
// Construct and run pick/place task
|
||||||
moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", nh);
|
moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh);
|
||||||
pick_place_task.loadParameters();
|
if (!pick_place_task.init()) {
|
||||||
pick_place_task.init();
|
ROS_INFO_NAMED(LOGNAME, "Initialization failed");
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
if (pick_place_task.plan()) {
|
if (pick_place_task.plan()) {
|
||||||
ROS_INFO_NAMED(LOGNAME, "Planning succeded");
|
ROS_INFO_NAMED(LOGNAME, "Planning succeded");
|
||||||
if (pnh.param("execute", false)) {
|
if (pnh.param("execute", false)) {
|
||||||
|
@ -38,9 +38,72 @@
|
|||||||
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
||||||
|
|
||||||
namespace moveit_task_constructor_demo {
|
namespace moveit_task_constructor_demo {
|
||||||
constexpr char LOGNAME[] = "pick_place_task";
|
|
||||||
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh)
|
constexpr char LOGNAME[] = "moveit_task_constructor_demo";
|
||||||
: nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {}
|
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<double> 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<double> 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() {
|
void PickPlaceTask::loadParameters() {
|
||||||
/****************************************************
|
/****************************************************
|
||||||
@ -49,45 +112,44 @@ void PickPlaceTask::loadParameters() {
|
|||||||
* *
|
* *
|
||||||
***************************************************/
|
***************************************************/
|
||||||
ROS_INFO_NAMED(LOGNAME, "Loading task parameters");
|
ROS_INFO_NAMED(LOGNAME, "Loading task parameters");
|
||||||
ros::NodeHandle pnh("~");
|
|
||||||
|
|
||||||
// Planning group properties
|
// Planning group properties
|
||||||
size_t errors = 0;
|
size_t errors = 0;
|
||||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_);
|
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_, "hand_group_name", hand_group_name_);
|
||||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_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_, "hand_frame", hand_frame_);
|
||||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_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_, "grasp_frame_transform", grasp_frame_transform_);
|
||||||
|
|
||||||
// Predefined pose targets
|
// Predefined pose targets
|
||||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_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_, "hand_close_pose", hand_close_pose_);
|
||||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_);
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_);
|
||||||
|
|
||||||
// Target object
|
// Target object
|
||||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_);
|
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_dimensions", object_dimensions_);
|
||||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_);
|
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_, "surface_link", surface_link_);
|
||||||
support_surfaces_ = { surface_link_ };
|
support_surfaces_ = { surface_link_ };
|
||||||
|
|
||||||
// Pick/Place metrics
|
// 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_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_, "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_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_, "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_surface_offset", place_surface_offset_);
|
||||||
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_);
|
errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_);
|
||||||
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
|
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PickPlaceTask::init() {
|
bool PickPlaceTask::init() {
|
||||||
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
|
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
|
||||||
const std::string object = object_name_;
|
const std::string object = object_name_;
|
||||||
|
|
||||||
// Reset ROS introspection before constructing the new object
|
// 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();
|
||||||
task_.reset(new moveit::task_constructor::Task());
|
task_.reset(new moveit::task_constructor::Task());
|
||||||
|
|
||||||
@ -414,36 +476,40 @@ void PickPlaceTask::init() {
|
|||||||
stage->restrictDirection(stages::MoveTo::FORWARD);
|
stage->restrictDirection(stages::MoveTo::FORWARD);
|
||||||
t.add(std::move(stage));
|
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<int>("max_solutions", 10);
|
|
||||||
|
|
||||||
|
// prepare Task structure for planning
|
||||||
try {
|
try {
|
||||||
task_->plan(max_solutions);
|
t.init();
|
||||||
} catch (InitStageException& e) {
|
} catch (InitStageException& e) {
|
||||||
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
|
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (task_->numSolutions() == 0) {
|
|
||||||
ROS_ERROR_NAMED(LOGNAME, "Planning failed");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool PickPlaceTask::plan() {
|
||||||
|
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
|
||||||
|
int max_solutions = pnh_.param<int>("max_solutions", 10);
|
||||||
|
|
||||||
|
return task_->plan(max_solutions);
|
||||||
|
}
|
||||||
|
|
||||||
bool PickPlaceTask::execute() {
|
bool PickPlaceTask::execute() {
|
||||||
ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory");
|
ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory");
|
||||||
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
|
moveit_msgs::MoveItErrorCodes execute_result;
|
||||||
task_->solutions().front()->fillMessage(execute_goal.solution);
|
|
||||||
execute_.sendGoal(execute_goal);
|
execute_result = task_->execute(*task_->solutions().front());
|
||||||
execute_.waitForResult();
|
// // If you want to inspect the goal message, use this instead:
|
||||||
moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code;
|
// actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>
|
||||||
|
// 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) {
|
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;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
13
demo/test/CMakeLists.txt
Normal file
13
demo/test/CMakeLists.txt
Normal file
@ -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_OBJECTS:pick_place_task>)
|
||||||
|
target_link_libraries(pick_place_test ${catkin_LIBRARIES} gtest_main)
|
||||||
|
|
||||||
|
add_rostest(pick_place.test)
|
||||||
|
endif()
|
15
demo/test/pick_place.test
Normal file
15
demo/test/pick_place.test
Normal file
@ -0,0 +1,15 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<launch>
|
||||||
|
<include file="$(find moveit_resources_panda_moveit_config)/launch/demo.launch">
|
||||||
|
<arg name="use_rviz" value="false" />
|
||||||
|
<arg name="execution_type" value="last point" />
|
||||||
|
<env name="LD_PRELOAD" value="$(optenv PRELOAD)" />
|
||||||
|
</include>
|
||||||
|
|
||||||
|
<!-- use MTC execution capability -->
|
||||||
|
<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
|
||||||
|
|
||||||
|
<test pkg="moveit_task_constructor_demo" type="pick_place_test" test-name="pick_place_test">
|
||||||
|
<rosparam command="load" file="$(find moveit_task_constructor_demo)/config/panda_config.yaml" />
|
||||||
|
</test>
|
||||||
|
</launch>
|
39
demo/test/pick_place_test.cpp
Normal file
39
demo/test/pick_place_test.cpp
Normal file
@ -0,0 +1,39 @@
|
|||||||
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||||
|
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||||
|
#include <moveit/task_constructor/stages/pick.h>
|
||||||
|
#include <moveit/task_constructor/stages/connect.h>
|
||||||
|
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
#include <moveit_task_constructor_demo/pick_place_task.h>
|
||||||
|
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
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();
|
||||||
|
}
|
@ -10,6 +10,17 @@ find_package(catkin REQUIRED COMPONENTS
|
|||||||
rviz
|
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
|
# definition needed for boost/math/constants/constants.hpp included by Ogre to compile
|
||||||
add_definitions(-DBOOST_MATH_DISABLE_FLOAT128)
|
add_definitions(-DBOOST_MATH_DISABLE_FLOAT128)
|
||||||
|
|
||||||
|
@ -151,9 +151,8 @@ rviz::Property* Parser::process(const yaml_event_t& event, const QString& name,
|
|||||||
case YAML_SCALAR_EVENT:
|
case YAML_SCALAR_EVENT:
|
||||||
return createScalar(name, description, byteArray(event), old);
|
return createScalar(name, description, byteArray(event), old);
|
||||||
default:
|
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.
|
// Try to set numeric or arbitrary scalar value from YAML node. Needs to match old's type.
|
||||||
|
@ -39,6 +39,9 @@ target_include_directories(${MOVEIT_LIB_NAME}
|
|||||||
PUBLIC $<TARGET_PROPERTY:moveit_task_visualization_tools,INTERFACE_INCLUDE_DIRECTORIES>
|
PUBLIC $<TARGET_PROPERTY:moveit_task_visualization_tools,INTERFACE_INCLUDE_DIRECTORIES>
|
||||||
PUBLIC ${catkin_INCLUDE_DIRS}
|
PUBLIC ${catkin_INCLUDE_DIRS}
|
||||||
)
|
)
|
||||||
|
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM
|
||||||
|
PUBLIC ${rviz_OGRE_INCLUDE_DIRS}
|
||||||
|
)
|
||||||
|
|
||||||
install(TARGETS ${MOVEIT_LIB_NAME}
|
install(TARGETS ${MOVEIT_LIB_NAME}
|
||||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
|
@ -42,9 +42,9 @@
|
|||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(BaseTaskModel)
|
MOVEIT_CLASS_FORWARD(BaseTaskModel);
|
||||||
MOVEIT_CLASS_FORWARD(TaskListModel)
|
MOVEIT_CLASS_FORWARD(TaskListModel);
|
||||||
MOVEIT_CLASS_FORWARD(TaskDisplay)
|
MOVEIT_CLASS_FORWARD(TaskDisplay);
|
||||||
|
|
||||||
/** MetaTaskListModel maintains a model of multiple registered TaskListModels,
|
/** MetaTaskListModel maintains a model of multiple registered TaskListModels,
|
||||||
* which are grouped in a hierarchical fashion according to the name of the
|
* which are grouped in a hierarchical fashion according to the name of the
|
||||||
|
@ -57,16 +57,16 @@ class RosTopicProperty;
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
namespace rdf_loader {
|
namespace rdf_loader {
|
||||||
MOVEIT_CLASS_FORWARD(RDFLoader)
|
MOVEIT_CLASS_FORWARD(RDFLoader);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
MOVEIT_CLASS_FORWARD(DisplaySolution);
|
||||||
class TaskListModel;
|
class TaskListModel;
|
||||||
|
|
||||||
class TaskDisplay : public rviz::Display
|
class TaskDisplay : public rviz::Display
|
||||||
|
@ -60,8 +60,8 @@ class DisplayContext;
|
|||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
MOVEIT_CLASS_FORWARD(DisplaySolution);
|
||||||
MOVEIT_CLASS_FORWARD(RemoteTaskModel)
|
MOVEIT_CLASS_FORWARD(RemoteTaskModel);
|
||||||
using StageFactory = PluginlibFactory<moveit::task_constructor::Stage>;
|
using StageFactory = PluginlibFactory<moveit::task_constructor::Stage>;
|
||||||
using StageFactoryPtr = std::shared_ptr<StageFactory>;
|
using StageFactoryPtr = std::shared_ptr<StageFactory>;
|
||||||
|
|
||||||
|
@ -155,9 +155,9 @@ void TaskPanel::request(rviz::WindowManagerInterface* window_manager) {
|
|||||||
if (SINGLETON || !vis_frame)
|
if (SINGLETON || !vis_frame)
|
||||||
return; // already defined, nothing to do
|
return; // already defined, nothing to do
|
||||||
|
|
||||||
QDockWidget* dock =
|
QDockWidget* dock = vis_frame->addPanelByName(
|
||||||
vis_frame->addPanelByName("Motion Planning Tasks", "moveit_task_constructor/Motion Planning Tasks",
|
"Motion Planning Tasks", "moveit_task_constructor/Motion Planning Tasks", Qt::LeftDockWidgetArea);
|
||||||
Qt::LeftDockWidgetArea, true /* floating */);
|
Q_UNUSED(dock);
|
||||||
assert(dock->widget() == SINGLETON);
|
assert(dock->widget() == SINGLETON);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -54,8 +54,8 @@ class EnumProperty;
|
|||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
class TaskSolutionVisualization;
|
class TaskSolutionVisualization;
|
||||||
MOVEIT_CLASS_FORWARD(TaskListModel)
|
MOVEIT_CLASS_FORWARD(TaskListModel);
|
||||||
MOVEIT_CLASS_FORWARD(TaskPanel)
|
MOVEIT_CLASS_FORWARD(TaskPanel);
|
||||||
|
|
||||||
/// Base class for all sub panels within the Task Panel
|
/// Base class for all sub panels within the Task Panel
|
||||||
class SubPanel : public QWidget
|
class SubPanel : public QWidget
|
||||||
|
@ -35,6 +35,9 @@ target_include_directories(${MOVEIT_LIB_NAME}
|
|||||||
PUBLIC include
|
PUBLIC include
|
||||||
PRIVATE ${catkin_INCLUDE_DIRS}
|
PRIVATE ${catkin_INCLUDE_DIRS}
|
||||||
)
|
)
|
||||||
|
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM
|
||||||
|
PUBLIC ${rviz_OGRE_INCLUDE_DIRS}
|
||||||
|
)
|
||||||
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
|
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
|
||||||
|
|
||||||
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
|
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
|
||||||
|
@ -41,14 +41,14 @@
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
}
|
}
|
||||||
namespace robot_trajectory {
|
namespace robot_trajectory {
|
||||||
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
MOVEIT_CLASS_FORWARD(RobotTrajectory);
|
||||||
}
|
}
|
||||||
namespace Ogre {
|
namespace Ogre {
|
||||||
class SceneNode;
|
class SceneNode;
|
||||||
@ -59,8 +59,8 @@ class DisplayContext;
|
|||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
MOVEIT_CLASS_FORWARD(DisplaySolution);
|
||||||
MOVEIT_CLASS_FORWARD(MarkerVisualization)
|
MOVEIT_CLASS_FORWARD(MarkerVisualization);
|
||||||
|
|
||||||
/** Class representing a task solution for display */
|
/** Class representing a task solution for display */
|
||||||
class DisplaySolution
|
class DisplaySolution
|
||||||
|
@ -17,17 +17,17 @@ class MarkerBase;
|
|||||||
} // namespace rviz
|
} // namespace rviz
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
}
|
}
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(MarkerVisualization)
|
MOVEIT_CLASS_FORWARD(MarkerVisualization);
|
||||||
|
|
||||||
/** Container for all markers created from a vector of Marker messages
|
/** Container for all markers created from a vector of Marker messages
|
||||||
*
|
*
|
||||||
|
@ -65,22 +65,22 @@ class PanelDockWidget;
|
|||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace core {
|
namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||||
}
|
}
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene)
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
}
|
}
|
||||||
namespace robot_trajectory {
|
namespace robot_trajectory {
|
||||||
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
MOVEIT_CLASS_FORWARD(RobotTrajectory);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(RobotStateVisualization)
|
MOVEIT_CLASS_FORWARD(RobotStateVisualization);
|
||||||
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
|
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization);
|
||||||
MOVEIT_CLASS_FORWARD(PlanningSceneRender)
|
MOVEIT_CLASS_FORWARD(PlanningSceneRender);
|
||||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
MOVEIT_CLASS_FORWARD(DisplaySolution);
|
||||||
|
|
||||||
class TaskSolutionPanel;
|
class TaskSolutionPanel;
|
||||||
class MarkerVisualizationProperty;
|
class MarkerVisualizationProperty;
|
||||||
|
Loading…
Reference in New Issue
Block a user