mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge CI fixes/improvements
This commit is contained in:
commit
64a8df1fde
31
.github/workflows/ci.yaml
vendored
31
.github/workflows/ci.yaml
vendored
@ -14,25 +14,26 @@ jobs:
|
||||
fail-fast: false
|
||||
matrix:
|
||||
env:
|
||||
- IMAGE: melodic-source
|
||||
- IMAGE: master-source
|
||||
TARGET_CMAKE_ARGS: >-
|
||||
-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="
|
||||
-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
|
||||
NAME: ccov
|
||||
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
|
||||
- IMAGE: master-source
|
||||
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
|
||||
TARGET_CMAKE_ARGS: >-
|
||||
-DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="
|
||||
-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 -g"
|
||||
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
|
||||
|
||||
env:
|
||||
CATKIN_LINT: true
|
||||
@ -51,16 +52,18 @@ jobs:
|
||||
name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v2
|
||||
- uses: actions/checkout@v3
|
||||
|
||||
- name: Cache ccache
|
||||
uses: pat-s/always-upload-cache@v2.1.5
|
||||
uses: rhaschke/cache@main
|
||||
with:
|
||||
path: ${{ env.CCACHE_DIR }}
|
||||
key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
|
||||
restore-keys: |
|
||||
ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
|
||||
ccache-${{ env.CACHE_PREFIX }}
|
||||
env:
|
||||
GHA_CACHE_SAVE: always
|
||||
|
||||
- id: ici
|
||||
name: Run industrial_ci
|
||||
@ -68,7 +71,7 @@ jobs:
|
||||
env: ${{ matrix.env }}
|
||||
|
||||
- name: Upload test artifacts (on failure)
|
||||
uses: actions/upload-artifact@v2
|
||||
uses: actions/upload-artifact@v3
|
||||
if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
|
||||
with:
|
||||
name: test-results-${{ matrix.env.IMAGE }}
|
||||
@ -81,7 +84,7 @@ jobs:
|
||||
workdir: ${{ env.BASEDIR }}/target_ws
|
||||
ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
|
||||
- name: Upload codecov report
|
||||
uses: codecov/codecov-action@v2
|
||||
uses: codecov/codecov-action@v3
|
||||
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
|
||||
with:
|
||||
files: ${{ env.BASEDIR }}/target_ws/coverage.info
|
||||
|
||||
5
.github/workflows/format.yaml
vendored
5
.github/workflows/format.yaml
vendored
@ -13,14 +13,13 @@ jobs:
|
||||
name: pre-commit
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v2
|
||||
- uses: actions/setup-python@v2
|
||||
- uses: actions/checkout@v3
|
||||
- name: Install clang-format-10
|
||||
run: sudo apt-get install clang-format-10
|
||||
- uses: rhaschke/install-catkin_lint-action@v1.0
|
||||
with:
|
||||
distro: noetic
|
||||
- uses: pre-commit/action@v2.0.0
|
||||
- uses: pre-commit/action@v3.0.0
|
||||
id: precommit
|
||||
- name: Upload pre-commit changes
|
||||
if: failure() && steps.precommit.outcome == 'failure'
|
||||
|
||||
@ -1,8 +1,6 @@
|
||||
cmake_minimum_required(VERSION 3.1.3)
|
||||
project(moveit_task_constructor_capabilities)
|
||||
|
||||
add_compile_options(-std=c++14)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
actionlib
|
||||
moveit_core
|
||||
@ -13,6 +11,8 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
std_msgs
|
||||
)
|
||||
|
||||
moveit_build_options()
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES $PROJECT_NAME}
|
||||
CATKIN_DEPENDS
|
||||
@ -21,14 +21,11 @@ catkin_package(
|
||||
std_msgs
|
||||
)
|
||||
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
src/execute_task_solution_capability.cpp
|
||||
)
|
||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}
|
||||
|
||||
@ -36,16 +36,12 @@
|
||||
|
||||
#include "execute_task_solution_capability.h"
|
||||
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/plan_execution/plan_execution.h>
|
||||
#include <moveit/trajectory_processing/trajectory_tools.h>
|
||||
#include <moveit/kinematic_constraints/utils.h>
|
||||
#include <moveit/move_group/capability_names.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
||||
#if MOVEIT_HAS_MESSAGE_CHECKS
|
||||
#include <moveit/utils/message_checks.h>
|
||||
#endif
|
||||
|
||||
namespace {
|
||||
|
||||
@ -170,22 +166,14 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
|
||||
/* TODO add action feedback and markers */
|
||||
exec_traj.effect_on_success_ = [this, sub_traj,
|
||||
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
|
||||
#if MOVEIT_HAS_MESSAGE_CHECKS
|
||||
if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
|
||||
#else
|
||||
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
|
||||
#endif
|
||||
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
|
||||
return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
|
||||
}
|
||||
return true;
|
||||
};
|
||||
|
||||
#if MOVEIT_HAS_MESSAGE_CHECKS
|
||||
if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) &&
|
||||
#else
|
||||
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
|
||||
#endif
|
||||
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) {
|
||||
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution",
|
||||
"invalid intermediate robot state in scene diff of SubTrajectory " << description);
|
||||
|
||||
@ -14,6 +14,9 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
rviz_marker_tools
|
||||
)
|
||||
|
||||
option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings
|
||||
moveit_build_options()
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES
|
||||
${PROJECT_NAME}
|
||||
@ -29,8 +32,6 @@ catkin_package(
|
||||
visualization_msgs
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
|
||||
add_compile_options(-fvisibility-inlines-hidden)
|
||||
|
||||
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
|
||||
|
||||
@ -44,24 +44,3 @@
|
||||
#define MOVEIT_VERSION_GE(major, minor, patch) \
|
||||
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
|
||||
major * 1'000'000 + minor * 1'000 + patch)
|
||||
|
||||
// use collision env instead of collision robot/world
|
||||
#define MOVEIT_HAS_COLLISION_ENV MOVEIT_VERSION_GE(1, 1, 0)
|
||||
|
||||
// cartesian interpolator got separated from RobotState at some point
|
||||
#define MOVEIT_HAS_CARTESIAN_INTERPOLATOR MOVEIT_VERSION_GE(1, 1, 0)
|
||||
|
||||
// isEmpty got split off into its own header
|
||||
#define MOVEIT_HAS_MESSAGE_CHECKS MOVEIT_VERSION_GE(1, 1, 0)
|
||||
|
||||
// use object shape poses relative to a single object pose
|
||||
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
|
||||
|
||||
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)
|
||||
|
||||
#if !MOVEIT_VERSION_GE(1, 1, 9)
|
||||
// the pointers are not yet available
|
||||
namespace trajectory_processing {
|
||||
MOVEIT_CLASS_FORWARD(TimeParameterization);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -140,9 +140,6 @@ private:
|
||||
Int i;
|
||||
};
|
||||
|
||||
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
||||
std::string frame);
|
||||
|
||||
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
|
||||
const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
|
||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
|
||||
|
||||
@ -39,8 +39,8 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||
PUBLIC ${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
|
||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_subdirectory(stages)
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/merge.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
|
||||
|
||||
|
||||
@ -36,7 +36,6 @@
|
||||
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/collision_detection/collision_common.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
@ -196,18 +195,9 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
|
||||
auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) {
|
||||
collision_detection::DistanceResult result;
|
||||
if (with_world)
|
||||
#if MOVEIT_HAS_COLLISION_ENV
|
||||
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
|
||||
#else
|
||||
state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(),
|
||||
robot);
|
||||
#endif
|
||||
else
|
||||
#if MOVEIT_HAS_COLLISION_ENV
|
||||
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
|
||||
#else
|
||||
state->scene()->getCollisionRobot()->distanceSelf(request, result, robot);
|
||||
#endif
|
||||
|
||||
if (result.minimum_distance.distance <= 0) {
|
||||
return result.minimum_distance;
|
||||
|
||||
@ -37,14 +37,10 @@
|
||||
*/
|
||||
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/trajectory_processing/time_parameterization.h>
|
||||
#include <moveit/kinematics_base/kinematics_base.h>
|
||||
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
|
||||
#include <moveit/robot_state/cartesian_interpolator.h>
|
||||
#endif
|
||||
|
||||
using namespace trajectory_processing;
|
||||
|
||||
@ -96,17 +92,11 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
|
||||
};
|
||||
|
||||
std::vector<moveit::core::RobotStatePtr> trajectory;
|
||||
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
|
||||
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
|
||||
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
|
||||
moveit::core::MaxEEFStep(props.get<double>("step_size")),
|
||||
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
|
||||
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"));
|
||||
#else
|
||||
double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath(
|
||||
jmg, trajectory, &link, target, true, props.get<double>("step_size"), props.get<double>("jump_threshold"),
|
||||
is_valid, props.get<kinematics::KinematicsQueryOptions>("kinematics_options"));
|
||||
#endif
|
||||
|
||||
assert(!trajectory.empty()); // there should be at least the start state
|
||||
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
*/
|
||||
|
||||
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/trajectory_processing/time_parameterization.h>
|
||||
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
*/
|
||||
|
||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
|
||||
|
||||
using namespace trajectory_processing;
|
||||
|
||||
@ -38,7 +38,6 @@
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -894,12 +893,10 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
||||
return false;
|
||||
}
|
||||
|
||||
#if MOVEIT_HAS_OBJECT_POSE
|
||||
if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) {
|
||||
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name);
|
||||
return false; // transforms do not match
|
||||
}
|
||||
#endif
|
||||
|
||||
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
|
||||
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name);
|
||||
@ -946,15 +943,9 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
||||
return false; // shapes not matching
|
||||
}
|
||||
|
||||
#if MOVEIT_HAS_OBJECT_POSE
|
||||
auto from_it = from_object->getShapePosesInLinkFrame().cbegin();
|
||||
auto from_end = from_object->getShapePosesInLinkFrame().cend();
|
||||
auto to_it = to_object->getShapePosesInLinkFrame().cbegin();
|
||||
#else
|
||||
auto from_it = from_object->getFixedTransforms().cbegin();
|
||||
auto from_end = from_object->getFixedTransforms().cend();
|
||||
auto to_it = to_object->getFixedTransforms().cbegin();
|
||||
#endif
|
||||
for (; from_it != from_end; ++from_it, ++to_it)
|
||||
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
|
||||
ROS_DEBUG_STREAM_NAMED("Connecting",
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/marker_tools.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
||||
@ -302,7 +301,7 @@ void ComputeIK::compute() {
|
||||
}
|
||||
ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
|
||||
|
||||
link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
|
||||
link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
|
||||
|
||||
// transform target pose such that ik frame will reach there if link does
|
||||
target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName());
|
||||
|
||||
@ -38,7 +38,6 @@
|
||||
|
||||
#include <moveit/task_constructor/stages/connect.h>
|
||||
#include <moveit/task_constructor/merge.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -38,7 +38,6 @@
|
||||
|
||||
#include <moveit/task_constructor/stages/fix_collision_objects.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
@ -117,13 +116,8 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
|
||||
bool failure = false;
|
||||
while (!failure) {
|
||||
res.clear();
|
||||
#if MOVEIT_HAS_COLLISION_ENV
|
||||
scene.getCollisionEnv()->checkRobotCollision(req, res, scene.getCurrentState(),
|
||||
scene.getAllowedCollisionMatrix());
|
||||
#else
|
||||
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(),
|
||||
scene.getCurrentState(), scene.getAllowedCollisionMatrix());
|
||||
#endif
|
||||
if (!res.collision)
|
||||
return result;
|
||||
|
||||
|
||||
@ -42,7 +42,6 @@
|
||||
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/task_constructor/utils.h>
|
||||
|
||||
#include <rviz_marker_tools/marker_creation.h>
|
||||
|
||||
@ -40,7 +40,6 @@
|
||||
#include <moveit/robot_state/robot_state.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
|
||||
@ -48,22 +47,6 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace utils {
|
||||
|
||||
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
||||
std::string frame) {
|
||||
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
|
||||
return state.getRigidlyConnectedParentLinkModel(frame);
|
||||
#else
|
||||
const moveit::core::LinkModel* link{ nullptr };
|
||||
|
||||
if (state.hasAttachedBody(frame)) {
|
||||
link = state.getAttachedBody(frame)->getAttachedLink();
|
||||
} else if (state.getRobotModel()->hasLinkModel(frame))
|
||||
link = state.getLinkModel(frame);
|
||||
|
||||
return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link);
|
||||
#endif
|
||||
}
|
||||
|
||||
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
|
||||
const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
|
||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) {
|
||||
@ -94,7 +77,7 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame;
|
||||
} else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) {
|
||||
robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id);
|
||||
robot_link = scene.getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame;
|
||||
} else {
|
||||
|
||||
@ -4,7 +4,6 @@
|
||||
#include <moveit/task_constructor/stages/move_relative.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -63,12 +62,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
|
||||
geometry_msgs::Pose p;
|
||||
p.position.x = 0.1;
|
||||
p.orientation.w = 1.0;
|
||||
#if MOVEIT_HAS_OBJECT_POSE
|
||||
aco.object.pose = p;
|
||||
#else
|
||||
aco.object.primitive_poses.resize(1, p);
|
||||
aco.object.primitive_poses[0] = p;
|
||||
#endif
|
||||
return aco;
|
||||
}
|
||||
|
||||
|
||||
@ -5,7 +5,6 @@
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -118,23 +117,14 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
|
||||
p.dimensions[p.SPHERE_RADIUS] = 0.01;
|
||||
return p;
|
||||
}());
|
||||
#if MOVEIT_HAS_OBJECT_POSE
|
||||
aco.object.pose.position.z = 0.2;
|
||||
aco.object.pose.orientation.w = 1.0;
|
||||
#else
|
||||
aco.object.primitive_poses.resize(1);
|
||||
aco.object.primitive_poses[0].position.z = 0.2;
|
||||
aco.object.primitive_poses[0].orientation.w = 1.0;
|
||||
#endif
|
||||
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
|
||||
// If we don't have this, we also don't have subframe support
|
||||
aco.object.subframe_names.resize(1, "subframe");
|
||||
aco.object.subframe_poses.resize(1, [] {
|
||||
geometry_msgs::Pose p;
|
||||
p.orientation.w = 1.0;
|
||||
return p;
|
||||
}());
|
||||
#endif
|
||||
return aco;
|
||||
}
|
||||
|
||||
@ -147,8 +137,6 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) {
|
||||
EXPECT_ONE_SOLUTION;
|
||||
}
|
||||
|
||||
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
|
||||
// If we don't have this, we also don't have subframe support
|
||||
TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
|
||||
const std::string ATTACHED_OBJECT{ "attached_object" };
|
||||
const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" };
|
||||
@ -159,7 +147,6 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
|
||||
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
|
||||
EXPECT_ONE_SOLUTION;
|
||||
}
|
||||
#endif
|
||||
|
||||
// This test require a running rosmaster
|
||||
TEST(Task, taskMoveConstructor) {
|
||||
|
||||
@ -1,12 +1,6 @@
|
||||
cmake_minimum_required(VERSION 3.1.3)
|
||||
project(moveit_task_constructor_demo)
|
||||
|
||||
if(NOT "${CMAKE_CXX_STANDARD}")
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
moveit_core
|
||||
@ -15,17 +9,16 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
rosparam_shortcuts
|
||||
)
|
||||
|
||||
moveit_build_options()
|
||||
|
||||
catkin_package(
|
||||
CATKIN_DEPENDS roscpp
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}_pick_place_task src/pick_place_task.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES})
|
||||
target_include_directories(${PROJECT_NAME}_pick_place_task PUBLIC include)
|
||||
target_include_directories(${PROJECT_NAME}_pick_place_task SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
|
||||
add_dependencies(${PROJECT_NAME}_pick_place_task ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
install(TARGETS ${PROJECT_NAME}_pick_place_task
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
@ -38,6 +31,7 @@ function(demo name)
|
||||
add_executable(${PROJECT_NAME}_${name} src/${name}.cpp)
|
||||
add_dependencies(${PROJECT_NAME}_${name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
target_link_libraries(${PROJECT_NAME}_${name} ${catkin_LIBRARIES})
|
||||
target_include_directories(${PROJECT_NAME}_${name} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
|
||||
set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "")
|
||||
install(TARGETS ${PROJECT_NAME}_${name}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
@ -56,7 +50,7 @@ demo(pick_place_demo)
|
||||
target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task)
|
||||
|
||||
install(DIRECTORY launch config
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
add_subdirectory(test)
|
||||
|
||||
@ -3,8 +3,6 @@
|
||||
#include <moveit/robot_model/robot_model.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/task_constructor/task.h>
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
@ -85,12 +83,7 @@ int main(int argc, char** argv) {
|
||||
co.id = "box";
|
||||
co.header.frame_id = "panda_link0";
|
||||
co.operation = co.ADD;
|
||||
#if MOVEIT_HAS_OBJECT_POSE
|
||||
auto& pose{ co.pose };
|
||||
#else
|
||||
co.primitive_poses.emplace_back();
|
||||
auto& pose{ co.primitive_poses[0] };
|
||||
#endif
|
||||
pose = []() {
|
||||
geometry_msgs::Pose p;
|
||||
p.position.x = 0.3;
|
||||
|
||||
@ -20,8 +20,6 @@ catkin_package(
|
||||
rviz
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
|
||||
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME})
|
||||
|
||||
set(HEADERS
|
||||
@ -34,10 +32,8 @@ add_library(${PROJECT_NAME}
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC include
|
||||
PRIVATE ${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
target_include_directories(${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>)
|
||||
target_include_directories(${PROJECT_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS})
|
||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
|
||||
|
||||
@ -10,6 +10,8 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
rviz
|
||||
)
|
||||
|
||||
moveit_build_options()
|
||||
|
||||
# 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)
|
||||
@ -57,8 +59,6 @@ catkin_package(
|
||||
rviz
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
|
||||
add_subdirectory(visualization_tools)
|
||||
add_subdirectory(motion_planning_tasks)
|
||||
|
||||
|
||||
@ -15,13 +15,9 @@ endif()
|
||||
|
||||
add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES})
|
||||
|
||||
target_link_libraries(${MOVEIT_LIB_NAME}
|
||||
${QT_LIBRARIES} ${YAML_LIBRARIES}
|
||||
)
|
||||
target_include_directories(${MOVEIT_LIB_NAME}
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
|
||||
PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS}
|
||||
)
|
||||
target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES} ${YAML_LIBRARIES})
|
||||
target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>)
|
||||
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS})
|
||||
|
||||
install(TARGETS ${MOVEIT_LIB_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
|
||||
@ -33,14 +33,14 @@ target_link_libraries(${MOVEIT_LIB_NAME}
|
||||
target_include_directories(${MOVEIT_LIB_NAME}
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
|
||||
PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
|
||||
# https://stackoverflow.com/questions/47175683/cmake-target-link-libraries-propagation-of-include-directories
|
||||
PUBLIC $<TARGET_PROPERTY:motion_planning_tasks_utils,INTERFACE_INCLUDE_DIRECTORIES>
|
||||
PUBLIC $<TARGET_PROPERTY:motion_planning_tasks_properties,INTERFACE_INCLUDE_DIRECTORIES>
|
||||
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}
|
||||
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PUBLIC
|
||||
# https://stackoverflow.com/questions/47175683/cmake-target-link-libraries-propagation-of-include-directories
|
||||
$<TARGET_PROPERTY:motion_planning_tasks_utils,INTERFACE_INCLUDE_DIRECTORIES>
|
||||
$<TARGET_PROPERTY:motion_planning_tasks_properties,INTERFACE_INCLUDE_DIRECTORIES>
|
||||
$<TARGET_PROPERTY:moveit_task_visualization_tools,INTERFACE_INCLUDE_DIRECTORIES>
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${rviz_OGRE_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
install(TARGETS ${MOVEIT_LIB_NAME}
|
||||
|
||||
@ -7,13 +7,9 @@ set(SOURCES
|
||||
)
|
||||
add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES})
|
||||
|
||||
target_link_libraries(${MOVEIT_LIB_NAME}
|
||||
${QT_LIBRARIES}
|
||||
)
|
||||
target_include_directories(${MOVEIT_LIB_NAME}
|
||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
|
||||
PRIVATE ${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES})
|
||||
target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>)
|
||||
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM PRIVATE ${catkin_INCLUDE_DIRS})
|
||||
|
||||
install(TARGETS ${MOVEIT_LIB_NAME}
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
|
||||
@ -31,12 +31,10 @@ target_link_libraries(${MOVEIT_LIB_NAME}
|
||||
${QT_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
target_include_directories(${MOVEIT_LIB_NAME}
|
||||
PUBLIC include
|
||||
PRIVATE ${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include)
|
||||
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM
|
||||
PUBLIC ${rviz_OGRE_INCLUDE_DIRS}
|
||||
PRIVATE ${catkin_INCLUDE_DIRS}
|
||||
PUBLIC ${rviz_OGRE_INCLUDE_DIRS}
|
||||
)
|
||||
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user