Merge branch 'master' into wip-python-api

This commit is contained in:
Robert Haschke 2021-06-11 09:52:54 +02:00
commit ced362f5f6
54 changed files with 396 additions and 235 deletions

View File

@ -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
View File

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

View File

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

View File

@ -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

View File

@ -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

View File

@ -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;

View File

@ -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

View File

@ -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

View File

@ -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
*

View File

@ -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

View File

@ -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

View File

@ -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&>;

View File

@ -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

View File

@ -45,7 +45,7 @@
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotState)
MOVEIT_CLASS_FORWARD(RobotState);
}
} // namespace moveit

View File

@ -46,7 +46,7 @@
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(JointModelGroup)
MOVEIT_CLASS_FORWARD(JointModelGroup);
}
} // namespace moveit

View File

@ -44,7 +44,7 @@ namespace moveit {
namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(CartesianPath)
MOVEIT_CLASS_FORWARD(CartesianPath);
}
namespace stages {

View File

@ -39,7 +39,7 @@
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotState)
MOVEIT_CLASS_FORWARD(RobotState);
}
} // namespace moveit

View File

@ -43,7 +43,7 @@
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(RobotModel);
}
} // namespace moveit
namespace moveit {

View File

@ -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
*

View File

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

View File

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

View File

@ -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">

View File

@ -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()

View File

@ -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

View File

@ -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>

View File

@ -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;
}

View File

@ -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})

View File

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

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

View File

@ -4,7 +4,7 @@
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(RobotModel);
}
} // namespace moveit

View File

@ -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 });

View File

@ -112,6 +112,7 @@ public:
auto impl{ this->pimpl() };
this->reset();
dummy->clear();
state_start.reset();
state_end.reset();

View File

@ -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);
}

View File

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

View File

@ -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_;

View File

@ -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>

View File

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

View File

@ -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
View 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
View 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>

View 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();
}

View File

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

View File

@ -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.

View File

@ -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}

View File

@ -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

View File

@ -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

View File

@ -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>;

View File

@ -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);
}

View File

@ -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

View File

@ -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})

View File

@ -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

View File

@ -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
*

View File

@ -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;