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
|
||||
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
|
||||
|
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
|
||||
${PROJECT_NAME}
|
||||
${PROJECT_NAME}_stages
|
||||
${PROJECT_NAME}_stage_plugins
|
||||
INCLUDE_DIRS
|
||||
include
|
||||
CATKIN_DEPENDS
|
||||
|
@ -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
|
||||
|
||||
|
@ -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<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
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
*
|
||||
|
@ -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
|
||||
|
@ -44,16 +44,16 @@
|
||||
#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
|
||||
|
||||
@ -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
|
||||
|
@ -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&>;
|
||||
|
||||
|
@ -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
|
||||
|
||||
|
@ -45,7 +45,7 @@
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
MOVEIT_CLASS_FORWARD(RobotState)
|
||||
MOVEIT_CLASS_FORWARD(RobotState);
|
||||
}
|
||||
} // namespace moveit
|
||||
|
||||
|
@ -46,7 +46,7 @@
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
MOVEIT_CLASS_FORWARD(JointModelGroup)
|
||||
MOVEIT_CLASS_FORWARD(JointModelGroup);
|
||||
}
|
||||
} // namespace moveit
|
||||
|
||||
|
@ -44,7 +44,7 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
|
||||
namespace solvers {
|
||||
MOVEIT_CLASS_FORWARD(CartesianPath)
|
||||
MOVEIT_CLASS_FORWARD(CartesianPath);
|
||||
}
|
||||
|
||||
namespace stages {
|
||||
|
@ -39,7 +39,7 @@
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
MOVEIT_CLASS_FORWARD(RobotState)
|
||||
MOVEIT_CLASS_FORWARD(RobotState);
|
||||
}
|
||||
} // namespace moveit
|
||||
|
||||
|
@ -43,7 +43,7 @@
|
||||
|
||||
namespace moveit {
|
||||
namespace core {
|
||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||
}
|
||||
} // namespace moveit
|
||||
namespace moveit {
|
||||
|
@ -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 @@ 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
|
||||
*
|
||||
|
@ -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) {
|
||||
|
@ -42,7 +42,7 @@
|
||||
#include <moveit/task_constructor/task.h>
|
||||
|
||||
namespace robot_model_loader {
|
||||
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
||||
MOVEIT_CLASS_FORWARD(RobotModelLoader);
|
||||
}
|
||||
|
||||
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"
|
||||
type="moveit::task_constructor::stages::CurrentState"
|
||||
base_class_type="moveit::task_constructor::Stage">
|
||||
|
@ -27,10 +27,14 @@ if(CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
|
||||
# Add folders to be run by python nosetests
|
||||
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()
|
||||
|
@ -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) {
|
||||
|
@ -1 +1 @@
|
||||
Subproject commit 3c92b9fbe6b05c254e7d20a7b9d8040c92029290
|
||||
Subproject commit 8f6ca71390f4cc1faf8e060146f347fa387ecd6a
|
@ -1,5 +1,5 @@
|
||||
<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_trampoline.py" test-name="rostest_trampoline" 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="60" args="" />
|
||||
</launch>
|
||||
|
@ -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<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 };
|
||||
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;
|
||||
}
|
||||
|
||||
|
@ -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})
|
||||
|
@ -96,7 +96,3 @@ void CurrentState::compute() {
|
||||
} // namespace stages
|
||||
} // namespace task_constructor
|
||||
} // 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 core {
|
||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||
MOVEIT_CLASS_FORWARD(RobotModel);
|
||||
}
|
||||
} // namespace moveit
|
||||
|
||||
|
@ -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<int> 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<int, int*, mtc::SolutionBasePtr, mtc::SolutionBaseConstPtr>;
|
||||
#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 });
|
||||
|
@ -112,6 +112,7 @@ public:
|
||||
auto impl{ this->pimpl() };
|
||||
this->reset();
|
||||
|
||||
dummy->clear();
|
||||
state_start.reset();
|
||||
state_end.reset();
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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_OBJECTS:pick_place_task>)
|
||||
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)
|
||||
|
@ -58,9 +58,6 @@
|
||||
#include <moveit/task_constructor/solvers/pipeline_planner.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>
|
||||
|
||||
#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<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> execute_;
|
||||
|
||||
// Pick metrics
|
||||
Eigen::Isometry3d grasp_frame_transform_;
|
||||
double approach_object_min_dist_;
|
||||
|
@ -11,10 +11,17 @@
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>moveit_task_constructor_core</depend>
|
||||
<depend>moveit_ros_planning_interface</depend>
|
||||
<depend>moveit_core</depend>
|
||||
|
||||
<depend>roscpp</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>
|
||||
|
||||
<test_depend>rostest</test_depend>
|
||||
</package>
|
||||
|
@ -40,83 +40,25 @@
|
||||
// MTC pick/place demo implementation
|
||||
#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";
|
||||
|
||||
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) {
|
||||
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)) {
|
||||
|
@ -38,9 +38,72 @@
|
||||
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
||||
|
||||
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<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() {
|
||||
/****************************************************
|
||||
@ -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<int>("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<int>("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<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) {
|
||||
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;
|
||||
}
|
||||
|
||||
|
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 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)
|
||||
|
||||
|
@ -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.
|
||||
|
@ -39,6 +39,9 @@ target_include_directories(${MOVEIT_LIB_NAME}
|
||||
PUBLIC $<TARGET_PROPERTY:moveit_task_visualization_tools,INTERFACE_INCLUDE_DIRECTORIES>
|
||||
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}
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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<moveit::task_constructor::Stage>;
|
||||
using StageFactoryPtr = std::shared_ptr<StageFactory>;
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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
|
||||
|
@ -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})
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
*
|
||||
|
@ -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;
|
||||
|
Loading…
Reference in New Issue
Block a user