diff --git a/.docker/README.md b/.docker/README.md
index a0cf726f20..b24af099ec 100644
--- a/.docker/README.md
+++ b/.docker/README.md
@@ -1,3 +1,3 @@
-# MoveIt! Docker Containers
+# MoveIt Docker Containers
For more information see [Continuous Integration and Docker](http://moveit.ros.org/documentation/contributing/continuous_integration.html) documentation.
diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile
index dd4c77c91f..830c8abd09 100644
--- a/.docker/ci/Dockerfile
+++ b/.docker/ci/Dockerfile
@@ -33,7 +33,7 @@ RUN \
clang clang-format-3.9 clang-tidy clang-tools \
ccache && \
#
- # Download all dependencies of MoveIt!
+ # Download all dependencies of MoveIt
rosdep update && \
rosdep install -y --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \
#
diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile
index e06998b1a2..4611c98b20 100644
--- a/.docker/release/Dockerfile
+++ b/.docker/release/Dockerfile
@@ -1,5 +1,5 @@
# moveit/moveit:melodic-release
-# Full debian-based install of MoveIt! using apt-get
+# Full debian-based install of MoveIt using apt-get
FROM ros:melodic-ros-base
MAINTAINER Dave Coleman dave@picknik.ai
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index a6d58c6912..ca05e3da91 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -1,4 +1,4 @@
-# MoveIt! is a large project with pull requests being created often. In order
+# MoveIt is a large project with pull requests being created often. In order
# to ensure quick review turn around time from our maintainer team, we're
# leveraging an automated "triage" approach to auto-assign reviews to new pull
# requests. If you already know who should review your PR, then you can assign
diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
index 81764f54b6..e653cc1ef4 100644
--- a/.github/PULL_REQUEST_TEMPLATE.md
+++ b/.github/PULL_REQUEST_TEMPLATE.md
@@ -4,11 +4,10 @@ Please explain the changes you made, including a reference to the related issue
### Checklist
- [ ] **Required by CI**: Code is auto formatted using [clang-format](http://moveit.ros.org/documentation/contributing/code)
-- [ ] Extended the tutorials / documentation, if necessary [reference](http://moveit.ros.org/documentation/contributing/)
+- [ ] Extend the tutorials / documentation [reference](http://moveit.ros.org/documentation/contributing/)
+- [ ] Document API changes relevant to the user in the MIGRATION.md notes
+- [ ] Create tests, which fail without this PR [reference](https://ros-planning.github.io/moveit_tutorials/)
- [ ] Include a screenshot if changing a GUI
-- [ ] Document API changes relevant to the user in the moveit/MIGRATION.md notes
-- [ ] Created tests, which fail without this PR [reference](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/tests.html)
-- [ ] Decide if this should be cherry-picked to other current ROS branches
-- [ ] While waiting for someone to review your request, please consider reviewing [another open pull request](https://github.com/ros-planning/moveit/pulls) to support the maintainers
+- [ ] While waiting for someone to review your request, please help review [another open pull request](https://github.com/ros-planning/moveit/pulls) to support the maintainers
[//]: # "You can expect a response from a maintainer within 7 days. If you haven't heard anything by then, feel free to ping the thread. Thank you!"
diff --git a/.github/config.yml b/.github/config.yml
index fed71ab355..2810cc56b6 100644
--- a/.github/config.yml
+++ b/.github/config.yml
@@ -11,12 +11,12 @@ newIssueWelcomeComment: >
# Comment to be posted to on PRs from first time contributors in your repository
newPRWelcomeComment: >
- Thanks for helping in improving MoveIt!
+ Thanks for helping in improving MoveIt
# Configuration for first-pr-merge - https://github.com/behaviorbot/first-pr-merge
# Comment to be posted to on pull requests merged by a first time user
firstPRMergeComment: >
- Congrats on getting your first MoveIt! pull request merged and improving open source robotics!
+ Congrats on getting your first MoveIt pull request merged and improving open source robotics!
# It is recommended to include as many gifs and emojis as possible!
diff --git a/.gitignore b/.gitignore
index 9877f9248d..808fe85341 100644
--- a/.gitignore
+++ b/.gitignore
@@ -51,10 +51,10 @@ qtcreator-*
# Vim
*.swp
-# Catkin custom files
-CATKIN_IGNORE
-
# Continous Integration
.moveit_ci
*.pyc
+
+#gdb
+*.gdb
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index eeafb30ddf..609c244cf6 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -1,4 +1,4 @@
-# Contributing to MoveIt!
+# Contributing to MoveIt
Thanks for getting involved! Information on contributing can be found at
[http://moveit.ros.org/documentation/contributing/](http://moveit.ros.org/documentation/contributing/)
diff --git a/MIGRATION.md b/MIGRATION.md
index 05740746a4..00cf53f4fb 100644
--- a/MIGRATION.md
+++ b/MIGRATION.md
@@ -1,6 +1,6 @@
# Migration Notes
-API changes in MoveIt! releases
+API changes in MoveIt releases
## ROS Noetic (upcoming changes in master)
- Extended the return value of `MoveitCommander.MoveGroup.plan()` from `trajectory` to a tuple of `(success, trajectory, planning_time, error_code)` to better match the C++ MoveGroupInterface ([790](https://github.com/ros-planning/moveit/pull/790/))
@@ -8,6 +8,7 @@ API changes in MoveIt! releases
- Moved the example package `moveit_controller_manager_example` into [moveit_tutorials](https://github.com/ros-planning/moveit_tutorials)
- Requests to `get_planning_scene` service without explicitly setting "components" now return full scene
- `moveit_ros_plannning` no longer depends on `moveit_ros_perception`
+- `CollisionRobot` and `CollisionWorld` are combined into a single `CollisionEnv` class. This applies for all derived collision checkers as `FCL`, `ALL_VALID`, `HYBRID` and `DISTANCE_FIELD`. Consequently, `getCollisionRobot[Unpadded] / getCollisionWorld` functions are replaced through a `getCollisionEnv` in the planning scene and return the new combined environment. This unified collision environment provides the union of all member functions of `CollisionRobot` and `CollisionWorld`. Note that calling `checkRobotCollision` of the `CollisionEnv` does not take a `CollisionRobot` as an argument anymore as it is implicitly contained in the `CollisionEnv`.
## ROS Melodic
diff --git a/README.md b/README.md
index 0ffd0e0e3b..0b330cc1bc 100644
--- a/README.md
+++ b/README.md
@@ -1,10 +1,10 @@
-
+
-The MoveIt! Motion Planning Framework
+The MoveIt Motion Planning Framework
-Currently we support ROS Indigo, Kinetic, and Melodic.
+Currently we support ROS Kinetic and Melodic for ROS 1.
-- [Overview of MoveIt!](http://moveit.ros.org)
+- [Overview of MoveIt](http://moveit.ros.org)
- [Installation Instructions](http://moveit.ros.org/install/)
- [Documentation](http://moveit.ros.org/documentation/)
- [Get Involved](http://moveit.ros.org/documentation/contributing/)
@@ -20,43 +20,52 @@ For MoveIt 2.0 development, see [moveit2](https://github.com/ros-planning/moveit
## Continuous Integration Status
-service | Indigo | Kinetic | Melodic | Master
----------- | ------ | ------- | ------- | ------
-Travis | [](https://travis-ci.org/ros-planning/moveit/branches) | [](https://travis-ci.org/ros-planning/moveit/branches) | [](https://travis-ci.org/ros-planning/moveit/branches) | [](https://travis-ci.org/ros-planning/moveit/branches) |
-build farm | [](http://build.ros.org/job/Idev__moveit__ubuntu_trusty_amd64) | [](http://build.ros.org/job/Kdev__moveit__ubuntu_xenial_amd64) | [](http://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | N/A |
+service | Kinetic | Melodic | Master
+---------- | ------- | ------- | ------
+Travis | [](https://travis-ci.org/ros-planning/moveit/branches) | [](https://travis-ci.org/ros-planning/moveit/branches) | [](https://travis-ci.org/ros-planning/moveit/branches) |
+build farm | [](http://build.ros.org/job/Kdev__moveit__ubuntu_xenial_amd64) | [](http://build.ros.org/job/Mdev__moveit__ubuntu_bionic_amd64) | N/A |
+## Licenses
+
+[](https://app.fossa.com/projects/git%2Bgithub.com%2Fros-planning%2Fmoveit?ref=badge_shield)
## Docker Containers
[](https://hub.docker.com/r/moveit/moveit/builds)
[](https://hub.docker.com/r/moveit/moveit/) [](https://hub.docker.com/r/moveit/moveit/) [](https://registry.hub.docker.com/moveit/moveit/)
+
## ROS Buildfarm
-MoveIt! Package | Indigo Source | Indigo Debian | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian
---------------- | ------------- | ------------- | -------------- | -------------- | -------------- | --------------
-moveit | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)
-moveit_chomp_optimizer_adapter | | | | | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)
-moveit_commander | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_commander__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_commander__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)
-moveit_core | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_core__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_core__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)
-moveit_experimental | | | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_experimental__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_experimental__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_experimental__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_experimental__ubuntu_bionic_amd64__binary)
-moveit_fake_controller_manager | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_fake_controller_manager__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_fake_controller_manager__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)
-moveit_kinematics | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_kinematics__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_kinematics__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_kinematics__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)
-moveit_planners | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_planners__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_planners__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)
-moveit_planners_chomp | | | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)
-moveit_planners_ompl | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_planners_ompl__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_planners_ompl__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)
-moveit_plugins | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_plugins__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_plugins__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_plugins__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)
-moveit_ros | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)
-moveit_ros_benchmarks | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_benchmarks__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_benchmarks__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)
-moveit_ros_control_interface | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_control_interface__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_control_interface__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)
-moveit_ros_manipulation | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_manipulation__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_manipulation__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)
-moveit_ros_move_group | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_move_group__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_move_group__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)
-moveit_ros_perception | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_perception__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_perception__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)
-moveit_ros_planning | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_planning__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_planning__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)
-moveit_ros_planning_interface | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_planning_interface__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_planning_interface__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)
-moveit_ros_robot_interaction | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_robot_interaction__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_robot_interaction__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)
-moveit_ros_visualization | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_visualization__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_visualization__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)
-moveit_ros_warehouse | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_ros_warehouse__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_ros_warehouse__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)
-moveit_runtime | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_runtime__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_runtime__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)
-moveit_setup_assistant | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_setup_assistant__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_setup_assistant__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)
-moveit_simple_controller_manager | [](http://build.ros.org/view/Isrc_uT/job/Isrc_uT__moveit_simple_controller_manager__ubuntu_trusty__source) | [](http://build.ros.org/view/Ibin_uT64/job/Ibin_uT64__moveit_simple_controller_manager__ubuntu_trusty_amd64__binary) | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)
-chomp_motion_planner | | | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)
+MoveIt Package | Kinetic Source | Kinetic Debian | Melodic Source | Melodic Debian
+--------------- | -------------- | -------------- | -------------- | --------------
+moveit | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit__ubuntu_bionic_amd64__binary)
+moveit_chomp_optimizer_adapter | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_chomp_optimizer_adapter__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_chomp_optimizer_adapter__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_chomp_optimizer_adapter__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_chomp_optimizer_adapter__ubuntu_bionic_amd64__binary)
+moveit_commander | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_commander__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_commander__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_commander__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_commander__ubuntu_bionic_amd64__binary)
+moveit_core | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_core__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_core__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_core__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_core__ubuntu_bionic_amd64__binary)
+moveit_experimental | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_experimental__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_experimental__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_experimental__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_experimental__ubuntu_bionic_amd64__binary)
+moveit_fake_controller_manager | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_fake_controller_manager__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_fake_controller_manager__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_fake_controller_manager__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_fake_controller_manager__ubuntu_bionic_amd64__binary)
+moveit_kinematics | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_kinematics__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_kinematics__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_kinematics__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_kinematics__ubuntu_bionic_amd64__binary)
+moveit_msgs | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_msgs__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_msgs__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_msgs__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_msgs__ubuntu_bionic_amd64__binary)
+moveit_planners | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners__ubuntu_bionic_amd64__binary)
+moveit_planners_chomp | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_chomp__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_chomp__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_chomp__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_chomp__ubuntu_bionic_amd64__binary)
+moveit_planners_ompl | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_planners_ompl__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_planners_ompl__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_planners_ompl__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_planners_ompl__ubuntu_bionic_amd64__binary)
+moveit_plugins | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_plugins__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_plugins__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_plugins__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_plugins__ubuntu_bionic_amd64__binary)
+moveit_resources | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_resources__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_resources__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_resources__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_resources__ubuntu_bionic_amd64__binary)
+moveit_ros | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros__ubuntu_bionic_amd64__binary)
+moveit_ros_benchmarks | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_benchmarks__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_benchmarks__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_benchmarks__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_benchmarks__ubuntu_bionic_amd64__binary)
+moveit_ros_control_interface | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_control_interface__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_control_interface__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_control_interface__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_control_interface__ubuntu_bionic_amd64__binary)
+moveit_ros_manipulation | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_manipulation__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_manipulation__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_manipulation__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_manipulation__ubuntu_bionic_amd64__binary)
+moveit_ros_move_group | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_move_group__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_move_group__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_move_group__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_move_group__ubuntu_bionic_amd64__binary)
+moveit_ros_perception | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_perception__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_perception__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_perception__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_perception__ubuntu_bionic_amd64__binary)
+moveit_ros_planning | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning__ubuntu_bionic_amd64__binary)
+moveit_ros_planning_interface | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_planning_interface__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_planning_interface__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_planning_interface__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_planning_interface__ubuntu_bionic_amd64__binary)
+moveit_ros_robot_interaction | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_robot_interaction__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_robot_interaction__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_robot_interaction__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_robot_interaction__ubuntu_bionic_amd64__binary)
+moveit_ros_visualization | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_visualization__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_visualization__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_visualization__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_visualization__ubuntu_bionic_amd64__binary)
+moveit_ros_warehouse | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_ros_warehouse__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_ros_warehouse__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_ros_warehouse__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_ros_warehouse__ubuntu_bionic_amd64__binary)
+moveit_runtime | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_runtime__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_runtime__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_runtime__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_runtime__ubuntu_bionic_amd64__binary)
+moveit_setup_assistant | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_setup_assistant__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_setup_assistant__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_setup_assistant__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_setup_assistant__ubuntu_bionic_amd64__binary)
+moveit_simple_controller_manager | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_simple_controller_manager__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_simple_controller_manager__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_simple_controller_manager__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_simple_controller_manager__ubuntu_bionic_amd64__binary)
+moveit_visual_tools | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__moveit_visual_tools__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__moveit_visual_tools__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__moveit_visual_tools__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__moveit_visual_tools__ubuntu_bionic_amd64__binary)
+chomp_motion_planner | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__chomp_motion_planner__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__chomp_motion_planner__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__chomp_motion_planner__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__chomp_motion_planner__ubuntu_bionic_amd64__binary)
+geometric_shapes | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__geometric_shapes__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__geometric_shapes__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__geometric_shapes__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__geometric_shapes__ubuntu_bionic_amd64__binary)
+srdfdom | [](http://build.ros.org/view/Ksrc_uX/job/Ksrc_uX__srdfdom__ubuntu_xenial__source) | [](http://build.ros.org/view/Kbin_uX64/job/Kbin_uX64__srdfdom__ubuntu_xenial_amd64__binary) | [](http://build.ros.org/view/Msrc_uB/job/Msrc_uB__srdfdom__ubuntu_bionic__source) | [](http://build.ros.org/view/Mbin_uB64/job/Mbin_uB64__srdfdom__ubuntu_bionic_amd64__binary)
\ No newline at end of file
diff --git a/moveit.rosinstall b/moveit.rosinstall
index f0ef21afa0..cca53db4e4 100644
--- a/moveit.rosinstall
+++ b/moveit.rosinstall
@@ -1,9 +1,9 @@
-# This file is intended for users who want to build MoveIt! from source.
-# Used with wstool, users can download source of all packages of MoveIt!.
+# This file is intended for users who want to build MoveIt from source.
+# Used with wstool, users can download source of all packages of MoveIt.
- git:
local-name: moveit_msgs
uri: https://github.com/ros-planning/moveit_msgs.git
- version: melodic-devel
+ version: master
- git:
local-name: geometric_shapes
uri: https://github.com/ros-planning/geometric_shapes.git
diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst
index 9f03a155e1..482cd91835 100644
--- a/moveit/CHANGELOG.rst
+++ b/moveit/CHANGELOG.rst
@@ -64,7 +64,7 @@ Changelog for package moveit
0.10.2 (2018-10-24)
-------------------
-* [fix] Text refrences to MoveIt! (`#1020 `_)
+* [fix] Text refrences to MoveIt (`#1020 `_)
* [fix] Eigen alignment issuses due to missing aligned allocation (`#1039 `_)
* [fix][chomp] changelogs: migration from tf -> tf2 only accidentally became part of 0.9.12's changelog
* [fix] Chomp package handling issue `#1086 `_ that was introduced in `ubi-agni/hotfix-#1012 `_
@@ -82,7 +82,7 @@ Changelog for package moveit
* [capability][chomp] Failure recovery options for CHOMP by tweaking parameters (`#987 `_)
* [capability] New screen for automatically generating interfaces to low level controllers(`#951 `_, `#994 `_, `#908 `_)
* [capability][moveit_setup_assistant] Perception screen for using laser scanner point clouds. (`#969 `_)
-* [enhancement][GUI][moveit_setup_assistant] Logo for MoveIt! 2.0, cleanup appearance (`#1059 `_)
+* [enhancement][GUI][moveit_setup_assistant] Logo for MoveIt 2.0, cleanup appearance (`#1059 `_)
* [enhancement][GUI][moveit_setup_assistant] added a setup assistant window icon (`#1028 `_)
* [capability][chomp] Addition of CHOMP planning adapter for optimizing result of other planners (`#1012 `_)
* [capability][chomp] Failure recovery options for CHOMP by tweaking parameters (`#987 `_)
diff --git a/moveit/package.xml b/moveit/package.xml
index 0a60b4860e..5a50dbd315 100644
--- a/moveit/package.xml
+++ b/moveit/package.xml
@@ -2,7 +2,7 @@
moveit
1.0.1
- Meta package that contains all essential package of MoveIt!. Until Summer 2016 MoveIt! had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories.
+ Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories.
Dave Coleman
Michael Ferguson
Michael Görner
diff --git a/moveit/scripts/README.md b/moveit/scripts/README.md
index ddc705b86f..e2da938586 100644
--- a/moveit/scripts/README.md
+++ b/moveit/scripts/README.md
@@ -1,8 +1,8 @@
-# Utility Scripts for MoveIt!
+# Utility Scripts for MoveIt
## README Markdown Buildfarm Table Generator
-This Python script will generate Github-style Markdown table with Jenkins badges for every package recursively from the current folder. It will order all packages alphabetically, but start with all MoveIt! packages first. For generating an updated README for a new ROS distribution, first update the `create_readme_table.py` script and add/remove the current MoveIt!-supported ROS distros and their corresponding Ubuntu distribution in the specified Python dictionary. Then, in the main MoveIt! `README.md` file, remove the "## ROS Buildfarm" header and the following table. Finally, from your catkin workspace with every MoveIt! package, run the `create_readme_table.py` script. For example:
+This Python script will generate Github-style Markdown table with Jenkins badges for every package recursively from the current folder. It will order all packages alphabetically, but start with all MoveIt packages first. For generating an updated README for a new ROS distribution, first update the `create_readme_table.py` script and add/remove the current MoveIt-supported ROS distros and their corresponding Ubuntu distribution in the specified Python dictionary. Then, in the main MoveIt `README.md` file, remove the "## ROS Buildfarm" header and the following table. Finally, from your catkin workspace with every MoveIt package, run the `create_readme_table.py` script. For example:
cd ~/ws_moveit/src
python moveit/moveit/scripts/create_readme_table.py >> moveit/README.md
diff --git a/moveit/scripts/create_maintainer_table.py b/moveit/scripts/create_maintainer_table.py
index af7615c2a9..1557530ee2 100644
--- a/moveit/scripts/create_maintainer_table.py
+++ b/moveit/scripts/create_maintainer_table.py
@@ -98,7 +98,7 @@ def populate_package_data(path, package):
def list_moveit_packages():
"""
- Creates MoveIt! List
+ Creates MoveIt List
"""
output = ''
packages = find_packages(os.getcwd())
diff --git a/moveit/scripts/create_readme_table.py b/moveit/scripts/create_readme_table.py
index 9a5af08554..9cbaed3e52 100644
--- a/moveit/scripts/create_readme_table.py
+++ b/moveit/scripts/create_readme_table.py
@@ -11,7 +11,7 @@
def create_header(ros_ubuntu_dict):
ros_distros = sorted(ros_ubuntu_dict.keys())
section_header = "## ROS Buildfarm\n"
- header="MoveIt! Package"
+ header="MoveIt Package"
header_lines = '-'*len(header)
for ros in ros_distros:
source = ' '.join([ros.capitalize(), "Source"])
@@ -49,13 +49,13 @@ def create_line(package, ros_ubuntu_dict):
def create_moveit_buildfarm_table():
"""
- Creates MoveIt! buildfarm badge table
+ Creates MoveIt buildfarm badge table
"""
# Update the following dictionary with the appropriate ROS-Ubuntu
# combinations for supported distribitions. For instance, in Noetic,
# remove {"indigo":"trusty"} and add {"noetic":"fbuntu"} with "fbuntu"
# being whatever the 20.04 distro is named
- supported_distro_ubuntu_dict = {"indigo":"trusty", "kinetic":"xenial", "melodic":"bionic"}
+ supported_distro_ubuntu_dict = {"kinetic":"xenial", "melodic":"bionic"}
all_packages = sorted([package.name for _, package in find_packages(os.getcwd()).items()])
moveit_packages = list()
diff --git a/moveit/scripts/maintainer_table_template.html b/moveit/scripts/maintainer_table_template.html
index b5c94f4020..584b1d68d6 100644
--- a/moveit/scripts/maintainer_table_template.html
+++ b/moveit/scripts/maintainer_table_template.html
@@ -2,7 +2,7 @@
- MoveIt! Package Tracking
+ MoveIt Package Tracking
@@ -23,7 +23,7 @@
- MoveIt! Package Tracking
+ MoveIt Package Tracking
| Package |
diff --git a/moveit_commander/CMakeLists.txt b/moveit_commander/CMakeLists.txt
index 70ba103732..74a0c315af 100644
--- a/moveit_commander/CMakeLists.txt
+++ b/moveit_commander/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.1.3)
project(moveit_commander)
find_package(catkin REQUIRED)
diff --git a/moveit_commander/README.md b/moveit_commander/README.md
index ecd51ed3b1..1b8f538576 100644
--- a/moveit_commander/README.md
+++ b/moveit_commander/README.md
@@ -1 +1 @@
-# MoveIt! Commander
+# MoveIt Commander
diff --git a/moveit_commander/bin/moveit_commander_cmdline.py b/moveit_commander/bin/moveit_commander_cmdline.py
index eb3c524c74..61a37e8898 100755
--- a/moveit_commander/bin/moveit_commander_cmdline.py
+++ b/moveit_commander/bin/moveit_commander_cmdline.py
@@ -4,7 +4,10 @@
import roslib
import rospy
-import readline
+try:
+ import readline
+except ImportError:
+ import pyreadline as readline # for Windows
import sys
import os
import signal
@@ -151,7 +154,7 @@ def sigint_handler(signal, frame):
rospy.init_node('move_group_interface_cmdline', anonymous=True, disable_signals=True)
parser = argparse.ArgumentParser(usage = """%(prog)s [options] []""",
- description = "Command Line Interface to MoveIt!")
+ description = "Command Line Interface to MoveIt")
parser.add_argument("-i", "--interactive", action="store_true", dest="interactive", default=True,
help="Run the command processing script in interactive mode (default)")
parser.add_argument("-s", "--service", action="store_true", dest="service", default=False,
diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml
index 6680232adc..a962c6a2df 100644
--- a/moveit_commander/package.xml
+++ b/moveit_commander/package.xml
@@ -7,7 +7,7 @@
Michael Görner
Robert Haschke
- MoveIt! Release Team
+ MoveIt Release Team
BSD
diff --git a/moveit_commander/src/moveit_commander/interpreter.py b/moveit_commander/src/moveit_commander/interpreter.py
index 3505e6deb5..02cc10e4d4 100644
--- a/moveit_commander/src/moveit_commander/interpreter.py
+++ b/moveit_commander/src/moveit_commander/interpreter.py
@@ -336,11 +336,11 @@ def execute_group_command(self, g, cmdorig):
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
- self._last_plan = g.plan()
+ self._last_plan = g.plan()[1] # The trajectory msg
else:
try:
g.set_named_target(clist[1])
- self._last_plan = g.plan()
+ self._last_plan = g.plan()[1] # The trajectory msg
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
except:
diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py
index a05c23cf85..0799e77c50 100644
--- a/moveit_commander/src/moveit_commander/move_group.py
+++ b/moveit_commander/src/moveit_commander/move_group.py
@@ -596,7 +596,7 @@ def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0,
traj_out.deserialize(ser_traj_out)
return traj_out
- def get_jacobian_matrix(self, joint_values):
+ def get_jacobian_matrix(self, joint_values, reference_point=None):
""" Get the jacobian matrix of the group as a list"""
- return self._g.get_jacobian_matrix(joint_values)
+ return self._g.get_jacobian_matrix(joint_values, [0.0, 0.0, 0.0] if reference_point is None else reference_point)
diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py
index a7fa6c9ad2..e0093e1508 100644
--- a/moveit_commander/src/moveit_commander/planning_scene_interface.py
+++ b/moveit_commander/src/moveit_commander/planning_scene_interface.py
@@ -69,7 +69,7 @@ def __init__(self, ns='', synchronous=False, service_timeout=5.0):
def __submit(self, collision_object, attach=False):
if self.__synchronous:
- diff_req = self.__make_planning_scene_diff_req(collision_object)
+ diff_req = self.__make_planning_scene_diff_req(collision_object, attach)
self._apply_planning_scene_diff.call(diff_req)
else:
if attach:
@@ -306,10 +306,14 @@ def __make_cylinder(name, pose, height, radius):
return co
@staticmethod
- def __make_planning_scene_diff_req(collision_object):
+ def __make_planning_scene_diff_req(collision_object, attach=False):
scene = PlanningScene()
scene.is_diff = True
- scene.world.collision_objects = [collision_object]
+ scene.robot_state.is_diff = True
+ if attach:
+ scene.robot_state.attached_collision_objects = [collision_object]
+ else:
+ scene.world.collision_objects = [collision_object]
planning_scene_diff_req = ApplyPlanningSceneRequest()
planning_scene_diff_req.scene = scene
return planning_scene_diff_req
diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt
index 9fd0c3bc6e..813c41187e 100644
--- a/moveit_core/CMakeLists.txt
+++ b/moveit_core/CMakeLists.txt
@@ -1,7 +1,8 @@
-cmake_minimum_required(VERSION 2.8.12)
+cmake_minimum_required(VERSION 3.1.3)
project(moveit_core)
-add_compile_options(-std=c++14)
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_EXTENSIONS OFF)
# Warnings
add_compile_options(-Wall -Wextra
@@ -174,7 +175,7 @@ string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_MINOR "${${PROJ
string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_PATCH "${${PROJECT_NAME}_VERSION}")
set(MOVEIT_VERSION_EXTRA "Alpha")
set(MOVEIT_VERSION "${MOVEIT_VERSION_MAJOR}.${MOVEIT_VERSION_MINOR}.${MOVEIT_VERSION_PATCH}-${MOVEIT_VERSION_EXTRA}")
-message(STATUS " *** Building MoveIt! ${MOVEIT_VERSION} ***")
+message(STATUS " *** Building MoveIt ${MOVEIT_VERSION} ***")
configure_file("version/version.h.in" "${VERSION_FILE_PATH}/moveit/version.h")
#catkin_lint: ignore_once external_file
install(FILES "${VERSION_FILE_PATH}/moveit/version.h" DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/moveit)
diff --git a/moveit_core/README.md b/moveit_core/README.md
index 2a54efedd4..5e602a457c 100644
--- a/moveit_core/README.md
+++ b/moveit_core/README.md
@@ -1,4 +1,4 @@
-# MoveIt! Core
+# MoveIt Core
This repository includes core libraries used by MoveIt:
- representation of kinematic models
diff --git a/moveit_core/background_processing/CMakeLists.txt b/moveit_core/background_processing/CMakeLists.txt
index 0e09ddc409..f0b7ab4712 100644
--- a/moveit_core/background_processing/CMakeLists.txt
+++ b/moveit_core/background_processing/CMakeLists.txt
@@ -7,6 +7,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
diff --git a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h
index 30cd7380ae..b87a82f997 100644
--- a/moveit_core/background_processing/include/moveit/background_processing/background_processing.h
+++ b/moveit_core/background_processing/include/moveit/background_processing/background_processing.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan */
-#ifndef MOVEIT_BACKGROUND_PROCESSING_
-#define MOVEIT_BACKGROUND_PROCESSING_
+#pragma once
#include
#include
@@ -47,7 +46,7 @@
namespace moveit
{
/** \brief This namespace includes classes and functions that are
- helpful in the implementation of other MoveIt! components. This is
+ helpful in the implementation of other MoveIt components. This is
not code specific to the functionality provided by MoveIt. */
namespace tools
{
@@ -113,5 +112,3 @@ class BackgroundProcessing : private boost::noncopyable
};
}
}
-
-#endif
diff --git a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h
index c00033f9bc..224c0fe2dc 100644
--- a/moveit_core/backtrace/include/moveit/backtrace/backtrace.h
+++ b/moveit_core/backtrace/include/moveit/backtrace/backtrace.h
@@ -32,8 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#ifndef MOVEIT_BACKTRACE_
-#define MOVEIT_BACKTRACE_
+#pragma once
#include
@@ -63,5 +62,3 @@ void get_backtrace(std::ostream& out)
}
#endif
}
-
-#endif
diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt
index bfc4e8db6b..262db3780d 100644
--- a/moveit_core/collision_detection/CMakeLists.txt
+++ b/moveit_core/collision_detection/CMakeLists.txt
@@ -1,15 +1,13 @@
set(MOVEIT_LIB_NAME moveit_collision_detection)
add_library(${MOVEIT_LIB_NAME}
- src/allvalid/collision_robot_allvalid.cpp
- src/allvalid/collision_world_allvalid.cpp
+ src/allvalid/collision_env_allvalid.cpp
src/collision_matrix.cpp
src/collision_octomap_filter.cpp
- src/collision_robot.cpp
src/collision_tools.cpp
- src/collision_world.cpp
src/world.cpp
src/world_diff.cpp
+ src/collision_env.cpp
)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
@@ -31,6 +29,7 @@ endif()
install(TARGETS ${MOVEIT_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h
index 549a6d6990..7782670035 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h
@@ -34,23 +34,18 @@
/* Author: Acorn Pooley, Ioan Sucan */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALL_VALID_H_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_ALL_VALID_H_
+#pragma once
#include
-#include
-#include
+#include
namespace collision_detection
{
/** \brief An allocator for AllValid collision detectors */
class CollisionDetectorAllocatorAllValid
- : public CollisionDetectorAllocatorTemplate
+ : public CollisionDetectorAllocatorTemplate
{
public:
- static const std::string NAME; // defined in collision_world_allvalid.cpp
+ static const std::string NAME; // defined in collision_env_allvalid.cpp
};
}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h
similarity index 55%
rename from moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h
rename to moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h
index c950579668..024a59496b 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_env_allvalid.h
@@ -32,48 +32,45 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/* Author: Ioan Sucan */
+/* Author: Ioan Sucan, Jia Pan, Jens Petit */
-#ifndef MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_WORLD_ALLVALID_
-#define MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_WORLD_ALLVALID_
+#pragma once
-#include
+#include
namespace collision_detection
{
-class CollisionRobotAllValid;
-
-class CollisionWorldAllValid : public CollisionWorld
+/** \brief Collision environment which always just returns no collisions.
+ *
+ * This can be used to save resources if collision checking is not important. */
+class CollisionEnvAllValid : public CollisionEnv
{
public:
- CollisionWorldAllValid();
- explicit CollisionWorldAllValid(const WorldPtr& world);
- CollisionWorldAllValid(const CollisionWorld& other, const WorldPtr& world);
+ CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0);
+ CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model, const WorldPtr& world, double padding = 0.0,
+ double scale = 1.0);
+ CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world);
- void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
+ void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const robot_state::RobotState& state) const override;
- void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override;
- void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override;
- void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const override;
-
- void checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionWorld& other_world) const override;
- void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
+ void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
const AllowedCollisionMatrix& acm) const override;
+ void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
+ const robot_state::RobotState& state2) const override;
+ void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
+ const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override;
- virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state) const;
- virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const;
- void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot,
+ virtual double distanceRobot(const robot_state::RobotState& state) const;
+ virtual double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
+ void distanceRobot(const DistanceRequest& req, DistanceResult& res,
const robot_state::RobotState& state) const override;
- virtual double distanceWorld(const CollisionWorld& world) const;
- virtual double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm) const;
- void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override;
+
+ void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const override;
+ void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
+ const AllowedCollisionMatrix& acm) const override;
+
+ void distanceSelf(const DistanceRequest& req, DistanceResult& res,
+ const robot_state::RobotState& state) const override;
};
}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h b/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h
deleted file mode 100644
index 9ac86aa6bb..0000000000
--- a/moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Ioan Sucan */
-
-#ifndef MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_ROBOT_ALLVALID_
-#define MOVEIT_COLLISION_DETECTION_ALLVALID_COLLISION_ROBOT_ALLVALID_
-
-#include
-
-namespace collision_detection
-{
-class CollisionRobotAllValid : public CollisionRobot
-{
-public:
- CollisionRobotAllValid(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0);
- CollisionRobotAllValid(const CollisionRobot& other);
-
- void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state) const override;
- void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const override;
- void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2) const override;
- void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override;
-
- void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const override;
- void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
- const CollisionRobot& other_robot, const robot_state::RobotState& other_state,
- const AllowedCollisionMatrix& acm) const override;
- void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state1,
- const robot_state::RobotState& other_state2) const override;
- void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state1, const robot_state::RobotState& other_state2,
- const AllowedCollisionMatrix& acm) const override;
-
- virtual double distanceSelf(const robot_state::RobotState& state) const;
- virtual double distanceSelf(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
- void distanceSelf(const DistanceRequest& req, DistanceResult& res,
- const robot_state::RobotState& state) const override;
-
- virtual double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const;
- virtual double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state, const AllowedCollisionMatrix& acm) const;
- void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state,
- const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override;
-};
-}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h
index 601755b0bb..2991951395 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_
+#pragma once
#include
#include
@@ -162,11 +161,10 @@ struct CollisionResult
/** \brief Number of contacts returned */
std::size_t contact_count;
- /** \brief A map returning the pairs of ids of the bodies in contact, plus information about the contacts themselves
- */
+ /** \brief A map returning the pairs of body ids in contact, plus their contact details */
ContactMap contacts;
- /** \brief When costs are computed, the individual cost sources are */
+ /** \brief These are the individual cost sources when costs are computed */
std::set cost_sources;
};
@@ -197,7 +195,7 @@ struct CollisionRequest
/** \brief If true, a collision cost is computed */
bool cost;
- /** \brief If true, compute contacts */
+ /** \brief If true, compute contacts. Otherwise only a binary collision yes/no is reported. */
bool contacts;
/** \brief Overall maximum number of contacts to compute */
@@ -224,14 +222,15 @@ namespace DistanceRequestTypes
{
enum DistanceRequestType
{
- GLOBAL, /// Find the global minimum
- SINGLE, /// Find the global minimum for each pair
- LIMITED, /// Find a limited(max_contacts_per_body) set of contacts for a given pair
- ALL /// Find all the contacts for a given pair
+ GLOBAL, ///< Find the global minimum
+ SINGLE, ///< Find the global minimum for each pair
+ LIMITED, ///< Find a limited(max_contacts_per_body) set of contacts for a given pair
+ ALL ///< Find all the contacts for a given pair
};
}
typedef DistanceRequestTypes::DistanceRequestType DistanceRequestType;
+/** \brief Representation of a distance-reporting request */
struct DistanceRequest
{
DistanceRequest()
@@ -264,7 +263,7 @@ struct DistanceRequest
/// Indicate the type of distance request. If using type=ALL, it is
/// recommended to set max_contacts_per_body to the expected number
- /// of contacts per pair becaused it is uesed to reserving space.
+ /// of contacts per pair because it is used to reserve space.
DistanceRequestType type;
/// Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold)
@@ -280,7 +279,7 @@ struct DistanceRequest
const AllowedCollisionMatrix* acm;
/// Only calculate distances for objects within this threshold to each other.
- /// If set this can significantly to reduce number of queries.
+ /// If set, this can significantly reduce the number of queries.
double distance_threshold;
/// Log debug information
@@ -291,6 +290,7 @@ struct DistanceRequest
bool compute_gradient;
};
+/** \brief Generic representation of the distance information for a pair of objects */
struct DistanceResultsData
{
DistanceResultsData()
@@ -356,8 +356,10 @@ struct DistanceResultsData
}
};
+/** \brief Mapping between the names of the collision objects and the DistanceResultData. */
typedef std::map, std::vector > DistanceMap;
+/** \brief Result of a distance request. */
struct DistanceResult
{
DistanceResult() : collision(false)
@@ -382,5 +384,3 @@ struct DistanceResult
}
};
}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h
index aed7eac019..cbfb39a161 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h
@@ -34,11 +34,9 @@
/* Author: Acorn Pooley, Ioan Sucan */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_
+#pragma once
-#include
-#include
+#include
#include
namespace collision_detection
@@ -57,22 +55,20 @@ class CollisionDetectorAllocator
virtual const std::string& getName() const = 0;
/** create a new CollisionWorld for checking collisions with the supplied world. */
- virtual CollisionWorldPtr allocateWorld(const WorldPtr& world) const = 0;
+ virtual CollisionEnvPtr allocateEnv(const WorldPtr& world,
+ const robot_model::RobotModelConstPtr& robot_model) const = 0;
/** create a new CollisionWorld by copying an existing CollisionWorld of the same type.s
* The world must be either the same world as used by \orig or a copy of that world which has not yet been modified.
*/
- virtual CollisionWorldPtr allocateWorld(const CollisionWorldConstPtr& orig, const WorldPtr& world) const = 0;
+ virtual CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const = 0;
- /** create a new CollisionRobot given a robot_model */
- virtual CollisionRobotPtr allocateRobot(const robot_model::RobotModelConstPtr& robot_model) const = 0;
-
- /** create a new CollisionRobot by copying an existing CollisionRobot of the same type. */
- virtual CollisionRobotPtr allocateRobot(const CollisionRobotConstPtr& orig) const = 0;
+ /** create a new CollisionEnv given a robot_model with a new empty world */
+ virtual CollisionEnvPtr allocateEnv(const robot_model::RobotModelConstPtr& robot_model) const = 0;
};
/** \brief Template class to make it easy to create an allocator for a specific CollisionWorld/CollisionRobot pair. */
-template
+template
class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator
{
public:
@@ -81,32 +77,25 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator
return CollisionDetectorAllocatorType::NAME;
}
- CollisionWorldPtr allocateWorld(const WorldPtr& world) const override
+ CollisionEnvPtr allocateEnv(const WorldPtr& world, const robot_model::RobotModelConstPtr& robot_model) const override
{
- return CollisionWorldPtr(new CollisionWorldType(world));
+ return CollisionEnvPtr(new CollisionEnvType(robot_model, world));
}
- CollisionWorldPtr allocateWorld(const CollisionWorldConstPtr& orig, const WorldPtr& world) const override
+ CollisionEnvPtr allocateEnv(const CollisionEnvConstPtr& orig, const WorldPtr& world) const override
{
- return CollisionWorldPtr(new CollisionWorldType(dynamic_cast(*orig), world));
+ return CollisionEnvPtr(new CollisionEnvType(dynamic_cast(*orig), world));
}
- CollisionRobotPtr allocateRobot(const robot_model::RobotModelConstPtr& robot_model) const override
+ CollisionEnvPtr allocateEnv(const robot_model::RobotModelConstPtr& robot_model) const override
{
- return CollisionRobotPtr(new CollisionRobotType(robot_model));
+ return CollisionEnvPtr(new CollisionEnvType(robot_model));
}
- CollisionRobotPtr allocateRobot(const CollisionRobotConstPtr& orig) const override
- {
- return CollisionRobotPtr(new CollisionRobotType(dynamic_cast(*orig)));
- }
-
- /** Create an allocator for FCL collision detectors */
+ /** Create an allocator for collision detectors. */
static CollisionDetectorAllocatorPtr create()
{
return CollisionDetectorAllocatorPtr(new CollisionDetectorAllocatorType());
}
};
}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h
similarity index 60%
rename from moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h
rename to moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h
index 1b6d476a89..c634ca8597 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h
@@ -14,7 +14,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of Willow Garage nor the names of its
+ * * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -32,44 +32,52 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/* Author: Ioan Sucan */
+/* Author: Ioan Sucan, Jens Petit */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_ROBOT_
+#pragma once
#include
#include
#include
#include
#include
+#include
namespace collision_detection
{
-MOVEIT_CLASS_FORWARD(CollisionRobot);
+MOVEIT_CLASS_FORWARD(CollisionEnv);
-/** @brief This class represents a collision model of the robot and can be used for self collision checks
- (to check if the robot is in collision with itself) or in collision checks with a different robot. Collision checks
- with
- the environment are performed using the CollisionWorld class. */
-class CollisionRobot
+/** \brief Provides the interface to the individual collision checking libraries. */
+class CollisionEnv
{
public:
+ CollisionEnv() = delete;
+
/** @brief Constructor
* @param model A robot model to construct the collision robot from
* @param padding The padding to use for all objects/links on the robot
* @scale scale A common scaling to use for all objects/links on the robot
*/
- CollisionRobot(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0);
+ CollisionEnv(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0);
- /** @brief A copy constructor*/
- CollisionRobot(const CollisionRobot& other);
+ /** @brief Constructor
+ * @param model A robot model to construct the collision robot from
+ * @param padding The padding to use for all objects/links on the robot
+ * @scale scale A common scaling to use for all objects/links on the robot
+ */
+ CollisionEnv(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0,
+ double scale = 1.0);
+
+ /** \brief Copy constructor */
+ CollisionEnv(const CollisionEnv& other, const WorldPtr& world);
- virtual ~CollisionRobot()
+ virtual ~CollisionEnv()
{
}
- /** @brief Check for self collision. Any collision between any pair of links is checked for, NO collisions are
+ /** @brief Check for robot self collision. Any collision between any pair of links is checked for, NO collisions are
* ignored.
+ *
* @param req A CollisionRequest object that encapsulates the collision request
* @param res A CollisionResult object that encapsulates the collision result
* @param state The kinematic state for which checks are being made */
@@ -85,82 +93,75 @@ class CollisionRobot
virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0;
- /** @brief Check for self collision in a continuous manner. Any collision between any pair of links is checked for,
- * NO collisions are ignored.
+ /** \brief Check whether the robot model is in collision with itself or the world at a particular state.
+ * Any collision between any pair of links is checked for, NO collisions are ignored.
* @param req A CollisionRequest object that encapsulates the collision request
* @param res A CollisionResult object that encapsulates the collision result
- * @param state1 The kinematic state at the start of the segment for which checks are being made
- * @param state2 The kinematic state at the end of the segment for which checks are being made */
- virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2) const = 0;
+ * @param state The kinematic state for which checks are being made */
+ virtual void checkCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const;
- /** \brief Check for self collision. Allowed collisions specified by the allowed collision matrix are
- * taken into account.
+ /** \brief Check whether the robot model is in collision with itself or the world at a particular state.
+ * Allowed collisions specified by the allowed collision matrix are taken into account.
* @param req A CollisionRequest object that encapsulates the collision request
* @param res A CollisionResult object that encapsulates the collision result
- * @param state1 The kinematic state at the start of the segment for which checks are being made
- * @param state2 The kinematic state at the end of the segment for which checks are being made
+ * @param state The kinematic state for which checks are being made
* @param acm The allowed collision matrix. */
- virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const = 0;
+ virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
+ const AllowedCollisionMatrix& acm) const;
- /** \brief Check for collision with a different robot (possibly a different kinematic model as well).
- * Any collision between any pair of links is checked for, NO collisions are ignored.
+ /** \brief Check whether the robot model is in collision with the world. Any collisions between a robot link
+ * and the world are considered. Self collisions are not checked.
* @param req A CollisionRequest object that encapsulates the collision request
* @param res A CollisionResult object that encapsulates the collision result
- * @param state The kinematic state for which checks are being made.
- * @param other_robot The collision representation for the other robot
- * @param other_state The kinematic state corresponding to the other robot */
- virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const = 0;
-
- /** \brief Check for collision with a different robot (possibly a different kinematic model as well).
- * Allowed collisions specified by the allowed collision matrix are taken into account.
+ * @robot robot The collision model for the robot
+ * @param state The kinematic state for which checks are being made
+ */
+ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const = 0;
+
+ /** \brief Check whether the robot model is in collision with the world.
+ * Allowed collisions are ignored. Self collisions are not checked.
* @param req A CollisionRequest object that encapsulates the collision request
* @param res A CollisionResult object that encapsulates the collision result
- * @param state The kinematic state for which checks are being made.
- * @param other_robot The collision representation for the other robot
- * @param other_state The kinematic state corresponding to the other robot
- * @param acm The allowed collision matrix. */
- virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state,
- const AllowedCollisionMatrix& acm) const = 0;
+ * @robot robot The collision model for the robot
+ * @param state The kinematic state for which checks are being made
+ * @param acm The allowed collision matrix.*/
+ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0;
- /** \brief Check for collision with a different robot (possibly a different kinematic model as well), in a continuous
- * fashion.
- * Any collision between any pair of links is checked for, NO collisions are ignored.
+ /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot
+ * states).
+ * Allowed collisions are ignored. Self collisions are not checked.
* @param req A CollisionRequest object that encapsulates the collision request
* @param res A CollisionResult object that encapsulates the collision result
- * @param state1 The kinematic state at the start of the segment for which checks are being made (this robot)
- * @param state2 The kinematic state at the end of the segment for which checks are being made (this robot)
- * @param other_robot The collision representation for the other robot
- * @param other_state1 The kinematic state at the start of the segment for which checks are being made (other robot)
- * @param other_state2 The kinematic state at the end of the segment for which checks are being made (other robot) */
- virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
+ * @robot robot The collision model for the robot
+ * @param state1 The kinematic state at the start of the segment for which checks are being made
+ * @param state2 The kinematic state at the end of the segment for which checks are being made
+ * @param acm The allowed collision matrix.*/
+ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const CollisionRobot& other_robot, const robot_state::RobotState& other_state1,
- const robot_state::RobotState& other_state2) const = 0;
+ const AllowedCollisionMatrix& acm) const = 0;
- /** \brief Check for collision with a different robot (possibly a different kinematic model as well), in a continuous
- * fashion.
- * Allowed collisions specified by the allowed collision matrix are taken into account.
+ /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot
+ * states).
+ * Allowed collisions are ignored. Self collisions are not checked.
* @param req A CollisionRequest object that encapsulates the collision request
* @param res A CollisionResult object that encapsulates the collision result
- * @param state1 The kinematic state at the start of the segment for which checks are being made (this robot)
- * @param state2 The kinematic state at the end of the segment for which checks are being made (this robot)
- * @param other_robot The collision representation for the other robot
- * @param other_state1 The kinematic state at the start of the segment for which checks are being made (other robot)
- * @param other_state2 The kinematic state at the end of the segment for which checks are being made (other robot)
- * @param acm The allowed collision matrix. */
- virtual void checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const CollisionRobot& other_robot, const robot_state::RobotState& other_state1,
- const robot_state::RobotState& other_state2,
- const AllowedCollisionMatrix& acm) const = 0;
+ * @robot robot The collision model for the robot
+ * @param state1 The kinematic state at the start of the segment for which checks are being made
+ * @param state2 The kinematic state at the end of the segment for which checks are being made
+ * @param acm The allowed collision matrix.*/
+ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state1,
+ const robot_state::RobotState& state2) const = 0;
+
+ /** \brief The distance to self-collision given the robot is at state \e state.
+ @param req A DistanceRequest object that encapsulates the distance request
+ @param res A DistanceResult object that encapsulates the distance result
+ @param state The state of this robot to consider */
+ virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res,
+ const robot_state::RobotState& state) const = 0;
/** \brief The distance to self-collision given the robot is at state \e state. */
inline double distanceSelf(const robot_state::RobotState& state) const
@@ -186,58 +187,69 @@ class CollisionRobot
return res.minimum_distance.distance;
}
- /** \brief The distance to self-collision given the robot is at state \e state.
- @param req A DistanceRequest object that encapsulates the distance request
- @param res A DistanceResult object that encapsulates the distance result
- @param state The state of this robot to consider */
- virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res,
- const robot_state::RobotState& state) const = 0;
-
- /** \brief The distance to another robot instance.
-
-
- @param state The state of this robot to consider
- @param other_robot The other robot instance to measure distance to
- @param other_state The state of the other robot */
- inline double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const
+ /** \brief Compute the distance between a robot and the world
+ * @param req A DistanceRequest object that encapsulates the distance request
+ * @param res A DistanceResult object that encapsulates the distance result
+ * @param robot The robot to check distance for
+ * @param state The state for the robot to check distances from */
+ virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res,
+ const robot_state::RobotState& state) const = 0;
+
+ /** \brief Compute the shortest distance between a robot and the world
+ * @param robot The robot to check distance for
+ * @param state The state for the robot to check distances from
+ * @param verbose Output debug information about distance checks */
+ inline double distanceRobot(const robot_state::RobotState& state, bool verbose = false) const
{
DistanceRequest req;
DistanceResult res;
+ req.verbose = verbose;
req.enableGroup(getRobotModel());
- distanceOther(req, res, state, other_robot, other_state);
+
+ distanceRobot(req, res, state);
return res.minimum_distance.distance;
}
- /** \brief The distance to another robot instance, ignoring distances between links that are allowed to always
- collide.
-
-
- @param state The state of this robot to consider
- @param other_robot The other robot instance to measure distance to
- @param other_state The state of the other robot
- @param acm The collision matrix specifying which links are allowed to always collide */
- inline double distanceOther(const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state, const AllowedCollisionMatrix& acm) const
+ /** \brief Compute the shortest distance between a robot and the world
+ * @param robot The robot to check distance for
+ * @param state The state for the robot to check distances from
+ * @param acm Using an allowed collision matrix has the effect of ignoring distances from links that are always
+ * allowed to be in collision.
+ * @param verbose Output debug information about distance checks */
+ inline double distanceRobot(const robot_state::RobotState& state, const AllowedCollisionMatrix& acm,
+ bool verbose = false) const
{
DistanceRequest req;
DistanceResult res;
- req.enableGroup(getRobotModel());
req.acm = &acm;
- distanceOther(req, res, state, other_robot, other_state);
+ req.verbose = verbose;
+ req.enableGroup(getRobotModel());
+
+ distanceRobot(req, res, state);
return res.minimum_distance.distance;
}
- /** \brief The distance to self-collision given the robot is at state \e state.
- @param req A DistanceRequest object that encapsulates the distance request
- @param res A DistanceResult object that encapsulates the distance result
- @param state The state of this robot to consider
- @param other_robot The other robot instance to measure distance to
- @param other_state The state of the other robot */
- virtual void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state,
- const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const = 0;
+ /** set the world to use.
+ * This can be expensive unless the new and old world are empty.
+ * Passing NULL will result in a new empty world being created. */
+ virtual void setWorld(const WorldPtr& world);
+
+ /** access the world geometry */
+ const WorldPtr& getWorld()
+ {
+ return world_;
+ }
+
+ /** access the world geometry */
+ const WorldConstPtr& getWorld() const
+ {
+ return world_const_;
+ }
+
+ typedef World::ObjectPtr ObjectPtr;
+ typedef World::ObjectConstPtr ObjectConstPtr;
/** @brief The kinematic model corresponding to this collision model*/
const robot_model::RobotModelConstPtr& getRobotModel() const
@@ -307,7 +319,9 @@ class CollisionRobot
/** @brief The internally maintained map (from link names to scaling)*/
std::map link_scale_;
+
+private:
+ WorldPtr world_; // The world always valid, never nullptr.
+ WorldConstPtr world_const_; // always same as world_
};
}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
index 59eb26e535..1b9e36e36e 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan, E. Gil Jones */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_MATRIX_
+#pragma once
#include
#include
@@ -278,5 +277,3 @@ class AllowedCollisionMatrix
std::map default_allowed_contacts_;
};
}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h
index 466d58a685..ed36a81113 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h
@@ -34,11 +34,10 @@
/* Author: Adam Leeper */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_OCTOMAP_FILTER_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_OCTOMAP_FILTER_
+#pragma once
#include
-#include
+#include
namespace collision_detection
{
@@ -62,5 +61,3 @@ int refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& r
double cell_bbx_search_distance = 1.0, double allowed_angle_divergence = 0.0,
bool estimate_depth = false, double iso_value = 0.5, double metaball_radius_multiple = 1.5);
}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h
index 156318f4bc..320265eb6b 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h
@@ -32,12 +32,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H
-#define MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H
+#pragma once
#include
-#include
-#include
+#include
#include
namespace collision_detection
@@ -53,8 +51,7 @@ MOVEIT_CLASS_FORWARD(CollisionPlugin);
* {
*
* class MyCollisionDetectorAllocator :
- * public collision_detection::CollisionDetectorAllocatorTemplate
+ * public collision_detection::CollisionDetectorAllocatorTemplate
* {
* public:
* static const std::string NAME_;
@@ -94,5 +91,3 @@ class CollisionPlugin
};
} // namespace collision_detection
-
-#endif // MOVEIT_COLLISION_DETECTION_COLLISION_PLUGIN_H
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h
index 6893c41444..8b06686148 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_TOOLS_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_TOOLS_
+#pragma once
#include
#include
@@ -72,5 +71,3 @@ bool getSensorPositioning(geometry_msgs::Point& point, const std::set
-
-#include
-#include
-#include
-#include
-
-/** \brief Generic interface to collision detection */
-namespace collision_detection
-{
-MOVEIT_CLASS_FORWARD(CollisionWorld);
-
-/** \brief Perform collision checking with the environment. The
- * collision world maintains a representation of the environment
- * that the robot is operating in. */
-class CollisionWorld : private boost::noncopyable
-{
-public:
- CollisionWorld();
-
- explicit CollisionWorld(const WorldPtr& world);
-
- /** \brief A copy constructor. \e other should not be changed while the copy constructor is running.
- * world must be the same world as used by other or a (not-yet-modified) copy of the world used by other */
- CollisionWorld(const CollisionWorld& other, const WorldPtr& world);
-
- virtual ~CollisionWorld()
- {
- }
-
- /**********************************************************************/
- /* Collision Checking Routines */
- /**********************************************************************/
-
- /** \brief Check whether the robot model is in collision with itself or the world at a particular state.
- * Any collision between any pair of links is checked for, NO collisions are ignored.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @param state The kinematic state for which checks are being made */
- virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state) const;
-
- /** \brief Check whether the robot model is in collision with itself or the world at a particular state.
- * Allowed collisions specified by the allowed collision matrix are taken into account.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @param state The kinematic state for which checks are being made
- * @param acm The allowed collision matrix. */
- virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
-
- /** \brief Check whether the robot model is in collision with itself or the world in a continuous manner
- * (between two robot states)
- * Any collision between any pair of links is checked for, NO collisions are ignored.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @param state1 The kinematic state at the start of the segment for which checks are being made
- * @param state2 The kinematic state at the end of the segment for which checks are being made */
- virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2) const;
-
- /** \brief Check whether the robot model is in collision with itself or the world in a continuous manner
- * (between two robot states).
- * Allowed collisions specified by the allowed collision matrix are taken into account.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @param state1 The kinematic state at the start of the segment for which checks are being made
- * @param state2 The kinematic state at the end of the segment for which checks are being made
- * @param acm The allowed collision matrix. */
- virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const;
-
- /** \brief Check whether the robot model is in collision with the world. Any collisions between a robot link
- * and the world are considered. Self collisions are not checked.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @robot robot The collision model for the robot
- * @param state The kinematic state for which checks are being made
- */
- virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state) const = 0;
-
- /** \brief Check whether the robot model is in collision with the world.
- * Allowed collisions are ignored. Self collisions are not checked.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @robot robot The collision model for the robot
- * @param state The kinematic state for which checks are being made
- * @param acm The allowed collision matrix.*/
- virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const = 0;
-
- /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot
- * states).
- * Any collisions between a robot link and the world are considered. Self collisions are not checked.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @robot robot The collision model for the robot
- * @param state1 The kinematic state at the start of the segment for which checks are being made
- * @param state2 The kinematic state at the end of the segment for which checks are being made */
- virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2) const = 0;
-
- /** \brief Check whether the robot model is in collision with the world in a continuous manner (between two robot
- * states).
- * Allowed collisions are ignored. Self collisions are not checked.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @robot robot The collision model for the robot
- * @param state1 The kinematic state at the start of the segment for which checks are being made
- * @param state2 The kinematic state at the end of the segment for which checks are being made
- * @param acm The allowed collision matrix.*/
- virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const = 0;
-
- /** \brief Check whether a given set of objects is in collision with objects from another world.
- * Any contacts are considered.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @param other_world The other collision world
- */
- virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionWorld& other_world) const = 0;
-
- /** \brief Check whether a given set of objects is in collision with objects from another world.
- * Allowed collisions are ignored. Any contacts are considered.
- * @param req A CollisionRequest object that encapsulates the collision request
- * @param res A CollisionResult object that encapsulates the collision result
- * @param other_world The other collision world
- * @param acm The allowed collision matrix.*/
- virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
- const AllowedCollisionMatrix& acm) const = 0;
-
- /** \brief Compute the shortest distance between a robot and the world
- * @param robot The robot to check distance for
- * @param state The state for the robot to check distances from
- * @param verbose Output debug information about distance checks */
- inline double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state,
- bool verbose = false) const
- {
- DistanceRequest req;
- DistanceResult res;
-
- req.verbose = verbose;
- req.enableGroup(robot.getRobotModel());
-
- distanceRobot(req, res, robot, state);
- return res.minimum_distance.distance;
- }
-
- /** \brief Compute the shortest distance between a robot and the world
- * @param robot The robot to check distance for
- * @param state The state for the robot to check distances from
- * @param acm Using an allowed collision matrix has the effect of ignoring distances from links that are always
- * allowed to be in collision.
- * @param verbose Output debug information about distance checks */
- inline double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm, bool verbose = false) const
- {
- DistanceRequest req;
- DistanceResult res;
-
- req.acm = &acm;
- req.verbose = verbose;
- req.enableGroup(robot.getRobotModel());
-
- distanceRobot(req, res, robot, state);
- return res.minimum_distance.distance;
- }
-
- /** \brief Compute the distance between a robot and the world
- * @param req A DistanceRequest object that encapsulates the distance request
- * @param res A DistanceResult object that encapsulates the distance result
- * @param robot The robot to check distance for
- * @param state The state for the robot to check distances from */
- virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state) const = 0;
-
- /** \brief The shortest distance to another world instance (\e world)
- * @param verbose Output debug information about distance checks */
- inline double distanceWorld(const CollisionWorld& world, bool verbose = false) const
- {
- DistanceRequest req;
- DistanceResult res;
-
- req.verbose = verbose;
- distanceWorld(req, res, world);
-
- return res.minimum_distance.distance;
- }
-
- /** \brief The shortest distance to another world instance (\e world), ignoring the distances between world elements
- * that are allowed to collide (as specified by \e acm)
- * @param verbose Output debug information about distance checks */
- inline double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm,
- bool verbose = false) const
- {
- DistanceRequest req;
- DistanceResult res;
-
- req.acm = &acm;
- req.verbose = verbose;
- distanceWorld(req, res, world);
-
- return res.minimum_distance.distance;
- }
-
- /** \brief Compute the distance between another world
- * @param req A DistanceRequest object that encapsulates the distance request
- * @param res A DistanceResult object that encapsulates the distance result
- * @param world The world to check distance for */
- virtual void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const = 0;
-
- /** set the world to use.
- * This can be expensive unless the new and old world are empty.
- * Passing NULL will result in a new empty world being created. */
- virtual void setWorld(const WorldPtr& world);
-
- /** access the world geometry */
- const WorldPtr& getWorld()
- {
- return world_;
- }
-
- /** access the world geometry */
- const WorldConstPtr& getWorld() const
- {
- return world_const_;
- }
-
- typedef World::ObjectPtr ObjectPtr;
- typedef World::ObjectConstPtr ObjectConstPtr;
-
-private:
- WorldPtr world_; // The world. Always valid. Never NULL.
- WorldConstPtr world_const_; // always same as world_
-};
-}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h
index f461043f4a..ad21d3a034 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h
@@ -34,8 +34,7 @@
/* Author: Ioan Sucan, Acorn Pooley, Sachin Chitta */
-#ifndef MOVEIT_COLLISION_DETECTION_WORLD_
-#define MOVEIT_COLLISION_DETECTION_WORLD_
+#pragma once
#include
@@ -288,7 +287,7 @@ class World
/** The objects maintained in the world */
std::map objects_;
- /* observers to call when something changes */
+ /** Wrapper for a callback function to call when something changes in the world */
class Observer
{
public:
@@ -297,8 +296,8 @@ class World
}
ObserverCallbackFn callback_;
};
+
+ /// All registered observers of this world representation
std::vector observers_;
};
}
-
-#endif
diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h
index 5f2013d7f0..84ae6a522c 100644
--- a/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h
+++ b/moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h
@@ -34,8 +34,7 @@
/* Author: Acorn Pooley, Ioan Sucan, Sachin Chitta */
-#ifndef MOVEIT_COLLISION_DETECTION_WORLD_DIFF_
-#define MOVEIT_COLLISION_DETECTION_WORLD_DIFF_
+#pragma once
#include
#include
@@ -126,5 +125,3 @@ class WorldDiff
WorldWeakPtr world_;
};
}
-
-#endif
diff --git a/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp
similarity index 53%
rename from moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp
rename to moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp
index 023f308a62..bf84110b23 100644
--- a/moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp
+++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp
@@ -32,118 +32,104 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/* Author: Ioan Sucan */
+/* Author: Ioan Sucan, Jia Pan, Jens Petit */
-#include
+#include
+#include
-collision_detection::CollisionWorldAllValid::CollisionWorldAllValid() : CollisionWorld()
-{
-}
+const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID");
-collision_detection::CollisionWorldAllValid::CollisionWorldAllValid(const WorldPtr& world) : CollisionWorld(world)
+collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model,
+ double padding, double scale)
+ : CollisionEnv(robot_model, padding, scale)
{
}
-collision_detection::CollisionWorldAllValid::CollisionWorldAllValid(const CollisionWorld& other, const WorldPtr& world)
- : CollisionWorld(other, world)
+collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const robot_model::RobotModelConstPtr& robot_model,
+ const WorldPtr& world, double padding, double scale)
+ : CollisionEnv(robot_model, world, padding, scale)
{
}
-void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot,
- const robot_state::RobotState& state) const
+collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world)
+ : CollisionEnv(other, world)
{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}
-void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot,
- const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const
+void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}
-void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2) const
+void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state,
+ const AllowedCollisionMatrix& acm) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}
-void collision_detection::CollisionWorldAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const
+void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state1,
+ const robot_state::RobotState& state2) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}
-void collision_detection::CollisionWorldAllValid::checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionWorld& other_world) const
+void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state1,
+ const robot_state::RobotState& state2,
+ const AllowedCollisionMatrix& acm) const
{
res.collision = false;
if (req.verbose)
ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}
-void collision_detection::CollisionWorldAllValid::checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionWorld& other_world,
- const AllowedCollisionMatrix& acm) const
+void collision_detection::CollisionEnvAllValid::distanceRobot(const collision_detection::DistanceRequest& req,
+ collision_detection::DistanceResult& res,
+ const moveit::core::RobotState& state) const
{
res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}
-double collision_detection::CollisionWorldAllValid::distanceRobot(const CollisionRobot& robot,
- const robot_state::RobotState& state) const
+double collision_detection::CollisionEnvAllValid::distanceRobot(const robot_state::RobotState& state) const
{
return 0.0;
}
-double collision_detection::CollisionWorldAllValid::distanceRobot(const CollisionRobot& robot,
- const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const
+double collision_detection::CollisionEnvAllValid::distanceRobot(const robot_state::RobotState& state,
+ const AllowedCollisionMatrix& acm) const
{
return 0.0;
}
-void collision_detection::CollisionWorldAllValid::distanceRobot(const collision_detection::DistanceRequest& req,
- collision_detection::DistanceResult& res,
- const collision_detection::CollisionRobot& robot,
- const moveit::core::RobotState& state) const
+void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const
{
res.collision = false;
+ if (req.verbose)
+ ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}
-double collision_detection::CollisionWorldAllValid::distanceWorld(const CollisionWorld& world) const
-{
- return 0.0;
-}
-
-double collision_detection::CollisionWorldAllValid::distanceWorld(const CollisionWorld& world,
- const AllowedCollisionMatrix& acm) const
+void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state,
+ const AllowedCollisionMatrix& acm) const
{
- return 0.0;
+ res.collision = false;
+ if (req.verbose)
+ ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
}
-void collision_detection::CollisionWorldAllValid::distanceWorld(const collision_detection::DistanceRequest& req,
- collision_detection::DistanceResult& res,
- const collision_detection::CollisionWorld& world) const
+void collision_detection::CollisionEnvAllValid::distanceSelf(const collision_detection::DistanceRequest& req,
+ collision_detection::DistanceResult& res,
+ const moveit::core::RobotState& state) const
{
res.collision = false;
}
-
-#include
-const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID");
diff --git a/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp
deleted file mode 100644
index 963f374711..0000000000
--- a/moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp
+++ /dev/null
@@ -1,171 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Ioan Sucan, Jia Pan */
-
-#include
-
-collision_detection::CollisionRobotAllValid::CollisionRobotAllValid(const robot_model::RobotModelConstPtr& robot_model,
- double padding, double scale)
- : CollisionRobot(robot_model, padding, scale)
-{
-}
-
-collision_detection::CollisionRobotAllValid::CollisionRobotAllValid(const CollisionRobot& other) : CollisionRobot(other)
-{
-}
-
-void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state) const
-{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
-}
-
-void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const
-{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
-}
-
-void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2) const
-{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
-}
-
-void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const
-{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
-}
-
-void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const
-{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
-}
-
-void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state,
- const AllowedCollisionMatrix& acm) const
-{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
-}
-
-void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state1,
- const robot_state::RobotState& other_state2) const
-{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
-}
-
-void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state1,
- const robot_state::RobotState& other_state2,
- const AllowedCollisionMatrix& acm) const
-{
- res.collision = false;
- if (req.verbose)
- ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
-}
-
-double collision_detection::CollisionRobotAllValid::distanceSelf(const robot_state::RobotState& state) const
-{
- return 0.0;
-}
-
-double collision_detection::CollisionRobotAllValid::distanceSelf(const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const
-{
- return 0.0;
-}
-
-void collision_detection::CollisionRobotAllValid::distanceSelf(const collision_detection::DistanceRequest& req,
- collision_detection::DistanceResult& res,
- const moveit::core::RobotState& state) const
-{
- res.collision = false;
-}
-
-double collision_detection::CollisionRobotAllValid::distanceOther(const robot_state::RobotState& state,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const
-{
- return 0.0;
-}
-
-double collision_detection::CollisionRobotAllValid::distanceOther(const robot_state::RobotState& state,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state,
- const AllowedCollisionMatrix& acm) const
-{
- return 0.0;
-}
-
-void collision_detection::CollisionRobotAllValid::distanceOther(const collision_detection::DistanceRequest& req,
- collision_detection::DistanceResult& res,
- const moveit::core::RobotState& state,
- const collision_detection::CollisionRobot& other_robot,
- const moveit::core::RobotState& other_state) const
-{
- res.collision = false;
-}
diff --git a/moveit_core/collision_detection/src/collision_robot.cpp b/moveit_core/collision_detection/src/collision_env.cpp
similarity index 66%
rename from moveit_core/collision_detection/src/collision_robot.cpp
rename to moveit_core/collision_detection/src/collision_env.cpp
index c3e27751a6..131b25676c 100644
--- a/moveit_core/collision_detection/src/collision_robot.cpp
+++ b/moveit_core/collision_detection/src/collision_env.cpp
@@ -14,7 +14,7 @@
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
- * * Neither the name of Willow Garage nor the names of its
+ * * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
@@ -32,9 +32,9 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/* Author: Ioan Sucan */
+/* Author: Ioan Sucan, Jens Petit */
-#include
+#include
#include
static inline bool validateScale(double scale)
@@ -69,8 +69,8 @@ static inline bool validatePadding(double padding)
namespace collision_detection
{
-CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, double padding, double scale)
- : robot_model_(model)
+CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, double padding, double scale)
+ : robot_model_(model), world_(new World()), world_const_(world_)
{
if (!validateScale(scale))
scale = 1.0;
@@ -85,13 +85,30 @@ CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, dou
}
}
-CollisionRobot::CollisionRobot(const CollisionRobot& other) : robot_model_(other.robot_model_)
+CollisionEnv::CollisionEnv(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding,
+ double scale)
+ : robot_model_(model), world_(world), world_const_(world_)
+{
+ if (!validateScale(scale))
+ scale = 1.0;
+ if (!validatePadding(padding))
+ padding = 0.0;
+
+ const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry();
+ for (auto link : links)
+ {
+ link_padding_[link->getName()] = padding;
+ link_scale_[link->getName()] = scale;
+ }
+}
+
+CollisionEnv::CollisionEnv(const CollisionEnv& other, const WorldPtr& world)
+ : robot_model_(other.robot_model_), world_(world), world_const_(world)
{
link_padding_ = other.link_padding_;
link_scale_ = other.link_scale_;
}
-
-void CollisionRobot::setPadding(double padding)
+void CollisionEnv::setPadding(double padding)
{
if (!validatePadding(padding))
return;
@@ -107,7 +124,7 @@ void CollisionRobot::setPadding(double padding)
updatedPaddingOrScaling(u);
}
-void CollisionRobot::setScale(double scale)
+void CollisionEnv::setScale(double scale)
{
if (!validateScale(scale))
return;
@@ -123,8 +140,9 @@ void CollisionRobot::setScale(double scale)
updatedPaddingOrScaling(u);
}
-void CollisionRobot::setLinkPadding(const std::string& link_name, double padding)
+void CollisionEnv::setLinkPadding(const std::string& link_name, double padding)
{
+ validatePadding(padding);
bool update = getLinkPadding(link_name) != padding;
link_padding_[link_name] = padding;
if (update)
@@ -134,7 +152,7 @@ void CollisionRobot::setLinkPadding(const std::string& link_name, double padding
}
}
-double CollisionRobot::getLinkPadding(const std::string& link_name) const
+double CollisionEnv::getLinkPadding(const std::string& link_name) const
{
auto it = link_padding_.find(link_name);
if (it != link_padding_.end())
@@ -143,11 +161,12 @@ double CollisionRobot::getLinkPadding(const std::string& link_name) const
return 0.0;
}
-void CollisionRobot::setLinkPadding(const std::map& padding)
+void CollisionEnv::setLinkPadding(const std::map& padding)
{
std::vector u;
for (const auto& link_pad_pair : padding)
{
+ validatePadding(link_pad_pair.second);
bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
link_padding_[link_pad_pair.first] = link_pad_pair.second;
if (update)
@@ -157,13 +176,14 @@ void CollisionRobot::setLinkPadding(const std::map& padding
updatedPaddingOrScaling(u);
}
-const std::map& CollisionRobot::getLinkPadding() const
+const std::map& CollisionEnv::getLinkPadding() const
{
return link_padding_;
}
-void CollisionRobot::setLinkScale(const std::string& link_name, double scale)
+void CollisionEnv::setLinkScale(const std::string& link_name, double scale)
{
+ validateScale(scale);
bool update = getLinkScale(link_name) != scale;
link_scale_[link_name] = scale;
if (update)
@@ -173,7 +193,7 @@ void CollisionRobot::setLinkScale(const std::string& link_name, double scale)
}
}
-double CollisionRobot::getLinkScale(const std::string& link_name) const
+double CollisionEnv::getLinkScale(const std::string& link_name) const
{
auto it = link_scale_.find(link_name);
if (it != link_scale_.end())
@@ -182,7 +202,7 @@ double CollisionRobot::getLinkScale(const std::string& link_name) const
return 1.0;
}
-void CollisionRobot::setLinkScale(const std::map& scale)
+void CollisionEnv::setLinkScale(const std::map& scale)
{
std::vector u;
for (const auto& link_scale_pair : scale)
@@ -196,16 +216,17 @@ void CollisionRobot::setLinkScale(const std::map& scale)
updatedPaddingOrScaling(u);
}
-const std::map& CollisionRobot::getLinkScale() const
+const std::map& CollisionEnv::getLinkScale() const
{
return link_scale_;
}
-void CollisionRobot::setPadding(const std::vector& padding)
+void CollisionEnv::setPadding(const std::vector& padding)
{
std::vector u;
for (const auto& p : padding)
{
+ validatePadding(p.padding);
bool update = getLinkPadding(p.link_name) != p.padding;
link_padding_[p.link_name] = p.padding;
if (update)
@@ -215,11 +236,12 @@ void CollisionRobot::setPadding(const std::vector& pad
updatedPaddingOrScaling(u);
}
-void CollisionRobot::setScale(const std::vector& scale)
+void CollisionEnv::setScale(const std::vector& scale)
{
std::vector u;
for (const auto& s : scale)
{
+ validateScale(s.scale);
bool update = getLinkScale(s.link_name) != s.scale;
link_scale_[s.link_name] = s.scale;
if (update)
@@ -229,7 +251,7 @@ void CollisionRobot::setScale(const std::vector& scale)
updatedPaddingOrScaling(u);
}
-void CollisionRobot::getPadding(std::vector& padding) const
+void CollisionEnv::getPadding(std::vector& padding) const
{
padding.clear();
for (const auto& lp : link_padding_)
@@ -241,7 +263,7 @@ void CollisionRobot::getPadding(std::vector& padding)
}
}
-void CollisionRobot::getScale(std::vector& scale) const
+void CollisionEnv::getScale(std::vector& scale) const
{
scale.clear();
for (const auto& ls : link_scale_)
@@ -253,8 +275,33 @@ void CollisionRobot::getScale(std::vector& scale) const
}
}
-void CollisionRobot::updatedPaddingOrScaling(const std::vector& links)
+void CollisionEnv::updatedPaddingOrScaling(const std::vector& links)
+{
+}
+
+void CollisionEnv::setWorld(const WorldPtr& world)
+{
+ world_ = world;
+ if (!world_)
+ world_.reset(new World());
+
+ world_const_ = world;
+}
+
+void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const
+{
+ checkSelfCollision(req, res, state);
+ if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
+ checkRobotCollision(req, res, state);
+}
+
+void CollisionEnv::checkCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const
{
+ checkSelfCollision(req, res, state, acm);
+ if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
+ checkRobotCollision(req, res, state, acm);
}
} // end of namespace collision_detection
diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp
index 5305727973..051a7afd25 100644
--- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp
+++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp
@@ -36,7 +36,6 @@
#include
#include
-#include
#include
#include
#include
diff --git a/moveit_core/collision_detection/src/collision_world.cpp b/moveit_core/collision_detection/src/collision_world.cpp
deleted file mode 100644
index ec2f3abcf9..0000000000
--- a/moveit_core/collision_detection/src/collision_world.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Ioan Sucan */
-
-#include
-#include
-
-namespace collision_detection
-{
-CollisionWorld::CollisionWorld() : world_(new World()), world_const_(world_)
-{
-}
-
-CollisionWorld::CollisionWorld(const WorldPtr& world) : world_(world), world_const_(world)
-{
-}
-
-CollisionWorld::CollisionWorld(const CollisionWorld& other, const WorldPtr& world) : world_(world), world_const_(world)
-{
-}
-
-void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state) const
-{
- robot.checkSelfCollision(req, res, state);
- if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
- checkRobotCollision(req, res, robot, state);
-}
-
-void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const
-{
- robot.checkSelfCollision(req, res, state, acm);
- if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
- checkRobotCollision(req, res, robot, state, acm);
-}
-
-void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2) const
-{
- robot.checkSelfCollision(req, res, state1, state2);
- if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
- checkRobotCollision(req, res, robot, state1, state2);
-}
-
-void CollisionWorld::checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const
-{
- robot.checkSelfCollision(req, res, state1, state2, acm);
- if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
- checkRobotCollision(req, res, robot, state1, state2, acm);
-}
-
-void CollisionWorld::setWorld(const WorldPtr& world)
-{
- world_ = world;
- if (!world_)
- world_.reset(new World());
-
- world_const_ = world;
-}
-
-} // end of namespace collision_detection
\ No newline at end of file
diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp
index 4fbf16e76c..f2a404ec8f 100644
--- a/moveit_core/collision_detection/src/world.cpp
+++ b/moveit_core/collision_detection/src/world.cpp
@@ -168,6 +168,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name) const
const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& frame_found) const
{
+ frame_found = true;
std::map::const_iterator it = objects_.find(name);
if (it != objects_.end())
return it->second->shape_poses_[0];
@@ -180,10 +181,7 @@ const Eigen::Isometry3d& World::getTransform(const std::string& name, bool& fram
{
auto it = object.second->subframe_poses_.find(name.substr(object.first.length() + 1));
if (it != object.second->subframe_poses_.end())
- {
- frame_found = true;
return it->second;
- }
}
}
}
diff --git a/moveit_core/collision_detection/test/test_all_valid.cpp b/moveit_core/collision_detection/test/test_all_valid.cpp
index 47a33a81cf..435f473980 100644
--- a/moveit_core/collision_detection/test/test_all_valid.cpp
+++ b/moveit_core/collision_detection/test/test_all_valid.cpp
@@ -36,14 +36,12 @@
#include
#include
-#include
-#include
+#include
TEST(AllValid, Instantiate)
{
using namespace collision_detection;
CollisionDetectorAllocatorAllValid allocator;
- CollisionWorldAllValid world;
urdf::ModelInterfaceSharedPtr urdf_model;
urdf_model.reset(new urdf::ModelInterface());
@@ -51,11 +49,11 @@ TEST(AllValid, Instantiate)
srdf_model.reset(new srdf::Model());
robot_model::RobotModelConstPtr robot_model;
robot_model.reset(new robot_model::RobotModel(urdf_model, srdf_model));
- CollisionRobotAllValid robot(robot_model);
+ CollisionEnvAllValid env(robot_model);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
-}
\ No newline at end of file
+}
diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt
index 8e73a17709..939f958d32 100644
--- a/moveit_core/collision_detection_fcl/CMakeLists.txt
+++ b/moveit_core/collision_detection_fcl/CMakeLists.txt
@@ -2,8 +2,7 @@ set(MOVEIT_LIB_NAME moveit_collision_detection_fcl)
add_library(${MOVEIT_LIB_NAME}
src/collision_common.cpp
- src/collision_robot_fcl.cpp
- src/collision_world_fcl.cpp
+ src/collision_env_fcl.cpp
)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
@@ -17,7 +16,8 @@ target_link_libraries(collision_detector_fcl_plugin ${catkin_LIBRARIES} ${MOVEIT
install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_fcl_plugin
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
@@ -29,4 +29,7 @@ if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.cpp)
target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES})
+
+ catkin_add_gtest(test_fcl_collision_env test/test_fcl_env.cpp)
+ target_link_libraries(test_fcl_collision_env moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES})
endif()
diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
index f9fce66b21..c5b89cca28 100644
--- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
+++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
@@ -34,11 +34,10 @@
/* Author: Ioan Sucan */
-#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_
-#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_
+#pragma once
#include
-#include
+#include
#include
#include
@@ -59,24 +58,29 @@ namespace collision_detection
{
MOVEIT_STRUCT_FORWARD(CollisionGeometryData);
+/** \brief Wrapper around world, link and attached objects' geometry data. */
struct CollisionGeometryData
{
+ /** \brief Constructor for a robot link collision geometry object. */
CollisionGeometryData(const robot_model::LinkModel* link, int index) : type(BodyTypes::ROBOT_LINK), shape_index(index)
{
ptr.link = link;
}
+ /** \brief Constructor for a new collision geometry object which is attached to the robot. */
CollisionGeometryData(const robot_state::AttachedBody* ab, int index)
: type(BodyTypes::ROBOT_ATTACHED), shape_index(index)
{
ptr.ab = ab;
}
+ /** \brief Constructor for a new world collision geometry. */
CollisionGeometryData(const World::Object* obj, int index) : type(BodyTypes::WORLD_OBJECT), shape_index(index)
{
ptr.obj = obj;
}
+ /** \brief Returns the name which is saved in the member pointed to in \e ptr. */
const std::string& getID() const
{
switch (type)
@@ -91,6 +95,7 @@ struct CollisionGeometryData
return ptr.obj->id_;
}
+ /** \brief Returns a string of the corresponding \e type. */
std::string getTypeString() const
{
switch (type)
@@ -105,14 +110,22 @@ struct CollisionGeometryData
return "Object";
}
- /** \brief Check if two CollisionGeometryData objects point to the same source object */
+ /** \brief Check if two CollisionGeometryData objects point to the same source object. */
bool sameObject(const CollisionGeometryData& other) const
{
return type == other.type && ptr.raw == other.ptr.raw;
}
+ /** \brief Indicates the body type of the object. */
BodyType type;
+
+ /** \brief Multiple \e CollisionGeometryData objects construct a collision object. The collision object refers to an
+ * array of coordinate transformations at a certain start index. The index of the transformation of a child \e
+ * CollisionGeometryData object is then given by adding the parent collision object index and the \e shape_index of a
+ * geometry data object. */
int shape_index;
+
+ /** \brief Points to the type of body which contains the geometry. */
union
{
const robot_model::LinkModel* link;
@@ -122,6 +135,7 @@ struct CollisionGeometryData
} ptr;
};
+/** \brief Data structure which is passed to the collision callback function of the collision manager. */
struct CollisionData
{
CollisionData() : req_(NULL), active_components_only_(NULL), res_(NULL), acm_(NULL), done_(false)
@@ -137,27 +151,29 @@ struct CollisionData
{
}
- /// Compute \e active_components_only_ based on \e req_
+ /** \brief Compute \e active_components_only_ based on the joint group specified in \e req_ */
void enableGroup(const robot_model::RobotModelConstPtr& robot_model);
- /// The collision request passed by the user
+ /** \brief The collision request passed by the user */
const CollisionRequest* req_;
- /// If the collision request includes a group name, this set contains the pointers to the link models that are
- /// considered for collision;
- /// If the pointer is NULL, all collisions are considered.
+ /** \brief If the collision request includes a group name, this set contains the pointers to the link models that
+ * are considered for collision.
+ *
+ * If the pointer is NULL, all collisions are considered. */
const std::set* active_components_only_;
- /// The user specified response location
+ /** \brief The user-specified response location. */
CollisionResult* res_;
- /// The user specified collision matrix (may be NULL)
+ /** \brief The user-specified collision matrix (may be NULL). */
const AllowedCollisionMatrix* acm_;
- /// Flag indicating whether collision checking is complete
+ /** \brief Flag indicating whether collision checking is complete. */
bool done_;
};
+/** \brief Data structure which is passed to the distance callback function of the collision manager. */
struct DistanceData
{
DistanceData(const DistanceRequest* req, DistanceResult* res) : req(req), res(res), done(false)
@@ -167,42 +183,48 @@ struct DistanceData
{
}
- /// Distance query request information
+ /** \brief Distance query request information. */
const DistanceRequest* req;
- /// Distance query results information
+ /** \brief Distance query results information. */
DistanceResult* res;
- /// Indicates if distance query is finished.
+ /** \brief Indicates if distance query is finished. */
bool done;
};
MOVEIT_STRUCT_FORWARD(FCLGeometry);
+/** \brief Bundles the \e CollisionGeometryData and FCL collision geometry representation into a single class. */
struct FCLGeometry
{
FCLGeometry()
{
}
+ /** \brief Constructor for a robot link. */
FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const robot_model::LinkModel* link, int shape_index)
: collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index))
{
collision_geometry_->setUserData(collision_geometry_data_.get());
}
+ /** \brief Constructor for an attached body. */
FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const robot_state::AttachedBody* ab, int shape_index)
: collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index))
{
collision_geometry_->setUserData(collision_geometry_data_.get());
}
+ /** \brief Constructor for a world object. */
FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const World::Object* obj, int shape_index)
: collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(obj, shape_index))
{
collision_geometry_->setUserData(collision_geometry_data_.get());
}
+ /** \brief Updates the \e collision_geometry_data_ with new data while also setting the \e collision_geometry_ to the
+ * new data. */
template
void updateCollisionGeometryData(const T* data, int shape_index, bool newType)
{
@@ -213,13 +235,18 @@ struct FCLGeometry
collision_geometry_->setUserData(collision_geometry_data_.get());
}
+ /** \brief Pointer to FCL collision geometry. */
std::shared_ptr collision_geometry_;
+
+ /** \brief Pointer to the user-defined geometry data. */
CollisionGeometryDataPtr collision_geometry_data_;
};
typedef std::shared_ptr FCLCollisionObjectPtr;
typedef std::shared_ptr FCLCollisionObjectConstPtr;
+/** \brief A general high-level object which consists of multiple \e FCLCollisionObjects. It is the top level data
+ * structure which is used in the collision checking process. */
struct FCLObject
{
void registerTo(fcl::BroadPhaseCollisionManagerd* manager);
@@ -227,33 +254,65 @@ struct FCLObject
void clear();
std::vector collision_objects_;
+
+ /** \brief Geometry data corresponding to \e collision_objects_. */
std::vector collision_geometry_;
};
+/** \brief Bundles an \e FCLObject and a broadphase FCL collision manager. */
struct FCLManager
{
FCLObject object_;
std::shared_ptr manager_;
};
+/** \brief Callback function used by the FCLManager used for each pair of collision objects to
+* calculate object contact information.
+*
+* \param o1 First FCL collision object
+* \param o2 Second FCL collision object
+* \data General pointer to arbitrary data which is used during the callback
+* \return True terminates the distance check, false continues it to the next pair of objects */
bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data);
+/** \brief Callback function used by the FCLManager used for each pair of collision objects to
+* calculate collisions and distances.
+*
+* \param o1 First FCL collision object
+* \param o2 Second FCL collision object
+* \data General pointer to arbitrary data which is used during the callback
+* \return True terminates the collision check, false continues it to the next pair of objects */
bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist);
+/** \brief Create new FCLGeometry object out of robot link model. */
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
int shape_index);
+
+/** \brief Create new FCLGeometry object out of attached body. */
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab,
int shape_index);
+
+/** \brief Create new FCLGeometry object out of a world object.
+ *
+ * A world object always consists only of a single shape, therefore we don't need the \e shape_index. */
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj);
+/** \brief Create new scaled and / or padded FCLGeometry object out of robot link model. */
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
const robot_model::LinkModel* link, int shape_index);
+
+/** \brief Create new scaled and / or padded FCLGeometry object out of an attached body. */
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
const robot_state::AttachedBody* ab, int shape_index);
+
+/** \brief Create new scaled and / or padded FCLGeometry object out of an world object. */
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
const World::Object* obj);
+
+/** \brief Increases the counter of the caches which can trigger the cleaning of expired entries from them. */
void cleanCollisionGeometryCache();
+/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */
inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f)
{
#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
@@ -265,6 +324,7 @@ inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f)
#endif
}
+/** \brief Transforms an Eigen Isometry3d to FCL coordinate transformation */
inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b)
{
fcl::Transform3d t;
@@ -272,6 +332,7 @@ inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b)
return t;
}
+/** \brief Transforms an FCL contact into a MoveIt contact point. */
inline void fcl2contact(const fcl::Contactd& fc, Contact& c)
{
c.pos = Eigen::Vector3d(fc.pos[0], fc.pos[1], fc.pos[2]);
@@ -285,6 +346,7 @@ inline void fcl2contact(const fcl::Contactd& fc, Contact& c)
c.body_type_2 = cgd2->type;
}
+/** \brief Transforms the FCL internal representation to the MoveIt \e CostSource data structure. */
inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs)
{
cs.aabb_min[0] = fcs.aabb_min[0];
@@ -298,5 +360,3 @@ inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs)
cs.cost = fcs.cost_density;
}
}
-
-#endif
diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h
index 6b7ec1ce52..88bcfcddae 100644
--- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h
+++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h
@@ -34,22 +34,18 @@
/* Author: Acorn Pooley, Ioan Sucan */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_FCL_H_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_FCL_H_
+#pragma once
#include
-#include
-#include
+#include
namespace collision_detection
{
/** \brief An allocator for FCL collision detectors */
class CollisionDetectorAllocatorFCL
- : public CollisionDetectorAllocatorTemplate
+ : public CollisionDetectorAllocatorTemplate
{
public:
- static const std::string NAME; // defined in collision_world_fcl.cpp
+ static const std::string NAME; // defined in collision_env_fcl.cpp
};
}
-
-#endif
diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h
index 18d7e8c40f..e1d749474c 100644
--- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h
+++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h
@@ -2,8 +2,7 @@
* collision_detector_fcl_plugin_loader.h
*/
-#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_
-#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_
+#pragma once
#include
#include
@@ -16,4 +15,3 @@ class CollisionDetectorFCLPluginLoader : public CollisionPlugin
virtual bool initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const;
};
}
-#endif // MOVEIT_COLLISION_DETECTION_FCL_COLLISION_DETECTOR_FCL_PLUGIN_LOADER_H_
\ No newline at end of file
diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h
new file mode 100644
index 0000000000..8906f8155f
--- /dev/null
+++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h
@@ -0,0 +1,163 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Ioan Sucan, Jens Petit */
+
+#pragma once
+
+#include
+#include
+
+#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
+#include
+#else
+#include
+#endif
+
+#include
+
+namespace collision_detection
+{
+/** \brief FCL implementation of the CollisionEnv */
+class CollisionEnvFCL : public CollisionEnv
+{
+public:
+ CollisionEnvFCL() = delete;
+
+ CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0);
+
+ CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0,
+ double scale = 1.0);
+
+ CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world);
+
+ ~CollisionEnvFCL() override;
+
+ virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const override;
+
+ virtual void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state,
+ const AllowedCollisionMatrix& acm) const override;
+
+ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const override;
+
+ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state,
+ const AllowedCollisionMatrix& acm) const override;
+
+ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state1, const robot_state::RobotState& state2,
+ const AllowedCollisionMatrix& acm) const override;
+
+ virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state1,
+ const robot_state::RobotState& state2) const override;
+
+ virtual void distanceSelf(const DistanceRequest& req, DistanceResult& res,
+ const robot_state::RobotState& state) const override;
+
+ virtual void distanceRobot(const DistanceRequest& req, DistanceResult& res,
+ const robot_state::RobotState& state) const override;
+
+ void setWorld(const WorldPtr& world) override;
+
+protected:
+ /** \brief Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a new
+ * padding or scaling of the robot links.
+ *
+ * It searches for the link through the pointed-to robot model of the CollisionRobot and then constructs new FCL
+ * collision objects and geometries depending on the changed robot model.
+ *
+ * \param links The names of the links which have been updated in the robot model */
+ void updatedPaddingOrScaling(const std::vector& links) override;
+
+ /** \brief Bundles the different checkSelfCollision functions into a single function */
+ void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
+ const AllowedCollisionMatrix* acm) const;
+
+ /** \brief Bundles the different checkRobotCollision functions into a single function */
+ void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const;
+
+ /** \brief Construct an FCL collision object from MoveIt's World::Object. */
+ void constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const;
+
+ /** \brief Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World.
+ *
+ * If it does not exist in world, it is deleted. If it's not existing in \m fcl_objs_ yet, it's added there. */
+ void updateFCLObject(const std::string& id);
+
+ /** \brief Out of the current robot state and its attached bodies construct an FCLObject which can then be used to
+ * check for collision.
+ *
+ * The current state is used to recalculate the AABB of the FCL collision objects. However they are not computed from
+ * scratch (which would require call to computeLocalAABB()) but are only transformed according to the joint states.
+ *
+ * \param state The current robot state
+ * \param fcl_obj The newly filled object */
+ void constructFCLObjectRobot(const robot_state::RobotState& state, FCLObject& fcl_obj) const;
+
+ /** \brief Prepares for the collision check through constructing an FCL collision object out of the current robot
+ * state and specifying a broadphase collision manager of FCL where the constructed object is registered to. */
+ void allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const;
+
+ /** \brief Converts all shapes which make up an atttached body into a vector of FCLGeometryConstPtr.
+ *
+ * When they are converted, they can be added to the FCL representation of the robot for collision checking.
+ *
+ * \param ab Pointer to the attached body
+ * \param geoms Output vector of geometries
+ */
+ void getAttachedBodyObjects(const robot_state::AttachedBody* ab, std::vector& geoms) const;
+
+ /** \brief Vector of shared pointers to the FCL geometry for the objects in fcl_objs_. */
+ std::vector robot_geoms_;
+
+ /** \brief Vector of shared pointers to the FCL collision objects which make up the robot */
+ std::vector robot_fcl_objs_;
+
+ /// FCL collision manager which handles the collision checking process
+ std::unique_ptr manager_;
+
+ std::map fcl_objs_;
+
+private:
+ /** \brief Callback function executed for each change to the world environment */
+ void notifyObjectChange(const ObjectConstPtr& obj, World::Action action);
+
+ World::ObserverHandle observer_handle_;
+};
+}
diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h
deleted file mode 100644
index a46794e999..0000000000
--- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Ioan Sucan */
-
-#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_ROBOT_
-#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_ROBOT_
-
-#include
-
-namespace collision_detection
-{
-class CollisionRobotFCL : public CollisionRobot
-{
- friend class CollisionWorldFCL;
-
-public:
- CollisionRobotFCL(const robot_model::RobotModelConstPtr& robot_model, double padding = 0.0, double scale = 1.0);
-
- CollisionRobotFCL(const CollisionRobotFCL& other);
-
- void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state) const override;
- void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const override;
- void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2) const override;
- void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2, const AllowedCollisionMatrix& acm) const override;
-
- void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const override;
- void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
- const CollisionRobot& other_robot, const robot_state::RobotState& other_state,
- const AllowedCollisionMatrix& acm) const override;
- void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state1,
- const robot_state::RobotState& other_state2) const override;
- void checkOtherCollision(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state1, const robot_state::RobotState& other_state2,
- const AllowedCollisionMatrix& acm) const override;
-
- void distanceSelf(const DistanceRequest& req, DistanceResult& res,
- const robot_state::RobotState& state) const override;
-
- void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state,
- const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override;
-
-protected:
- void updatedPaddingOrScaling(const std::vector& links) override;
- void constructFCLObject(const robot_state::RobotState& state, FCLObject& fcl_obj) const;
- void allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const;
- void getAttachedBodyObjects(const robot_state::AttachedBody* ab, std::vector& geoms) const;
-
- void checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res, const robot_state::RobotState& state,
- const AllowedCollisionMatrix* acm) const;
- void checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state, const AllowedCollisionMatrix* acm) const;
-
- std::vector geoms_;
- std::vector fcl_objs_;
-};
-}
-
-#endif
diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h
deleted file mode 100644
index e6d9557630..0000000000
--- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h
+++ /dev/null
@@ -1,101 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Ioan Sucan */
-
-#ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_WORLD_FCL_
-#define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_WORLD_FCL_
-
-#include
-#include
-
-#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
-#include
-#else
-#include
-#endif
-
-#include
-
-namespace collision_detection
-{
-class CollisionWorldFCL : public CollisionWorld
-{
-public:
- CollisionWorldFCL();
- explicit CollisionWorldFCL(const WorldPtr& world);
- CollisionWorldFCL(const CollisionWorldFCL& other, const WorldPtr& world);
- ~CollisionWorldFCL() override;
-
- void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state) const override;
- void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const override;
- void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2) const override;
- void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const override;
- void checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionWorld& other_world) const override;
- void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
- const AllowedCollisionMatrix& acm) const override;
-
- void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state) const override;
-
- void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override;
-
- void setWorld(const WorldPtr& world) override;
-
-protected:
- void checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
- const AllowedCollisionMatrix* acm) const;
- void checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state, const AllowedCollisionMatrix* acm) const;
-
- void constructFCLObject(const World::Object* obj, FCLObject& fcl_obj) const;
- void updateFCLObject(const std::string& id);
-
- std::unique_ptr manager_;
- std::map fcl_objs_;
-
-private:
- void initialize();
- void notifyObjectChange(const ObjectConstPtr& obj, World::Action action);
- World::ObserverHandle observer_handle_;
-};
-}
-
-#endif
diff --git a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h
index 9eeda077cd..123fac3c08 100644
--- a/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h
+++ b/moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/fcl_compat.h
@@ -34,8 +34,7 @@
/* Author: Benjamin Scholz */
-#ifndef MOVEIT_COLLISION_DETECTION_FCL_COMPAT_
-#define MOVEIT_COLLISION_DETECTION_FCL_COMPAT_
+#pragma once
#include
@@ -98,4 +97,3 @@ class DynamicAABBTreeCollisionManager;
using DynamicAABBTreeCollisionManagerd = fcl::DynamicAABBTreeCollisionManager;
}
#endif
-#endif
diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp
index e977b025a2..0cf3446237 100644
--- a/moveit_core/collision_detection_fcl/src/collision_common.cpp
+++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp
@@ -356,6 +356,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
return cdata->done_;
}
+/** \brief Cache for an arbitrary type of shape. It is assigned during the execution of \e createCollisionGeometry().
+ *
+ * Only a single cache per thread and object type is created as it is a quasi-singleton instance. */
struct FCLShapeCache
{
using ShapeKey = shapes::ShapeConstWeakPtr;
@@ -390,7 +393,10 @@ struct FCLShapeCache
static const unsigned int MAX_CLEAN_COUNT = 100; // every this many uses of the cache, a cleaning operation is
// executed (this is only removal of expired entries)
+ /** \brief Map of weak pointers to the FCLGeometry. */
ShapeMap map_;
+
+ /** \brief Counts cache usage and triggers clearing of cache when \m MAX_CLEAN_COUNT is exceeded. */
unsigned int clean_count_;
};
@@ -624,7 +630,9 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
return cdata->done;
}
-/* We template the function so we get a different cache for each of the template arguments combinations */
+/* Templated function to get a different cache for each of the template arguments combinations.
+ *
+ * The returned cache is a quasi-singleton for each thread as it is created \e thread_local. */
template
FCLShapeCache& GetShapeCache()
{
@@ -659,6 +667,11 @@ struct IfSameType
};
};
+/** \brief Templated helper function creating new collision geometry out of general object using an arbitrary bounding
+ * volume (BV).
+ *
+ * It assigns a thread-local cache for each type of shape and minimizes memory usage and copying through utilizing the
+ * cache. */
template
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const T* data, int shape_index)
{
@@ -839,7 +852,6 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape,
return FCLGeometryConstPtr();
}
-/////////////////////////////////////////////////////
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
int shape_index)
{
@@ -857,6 +869,8 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape,
return createCollisionGeometry(shape, obj, 0);
}
+/** \brief Templated helper function creating new collision geometry out of general object using an arbitrary bounding
+ * volume (BV). This can include padding and scaling. */
template
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
const T* data, int shape_index)
@@ -901,9 +915,8 @@ void cleanCollisionGeometryCache()
cache2.bumpUseCount(true);
}
}
-} // namespace collision_detection
-void collision_detection::CollisionData::enableGroup(const robot_model::RobotModelConstPtr& robot_model)
+void CollisionData::enableGroup(const robot_model::RobotModelConstPtr& robot_model)
{
if (robot_model->hasJointModelGroup(req_->group_name))
active_components_only_ = &robot_model->getJointModelGroup(req_->group_name)->getUpdatedLinkModelsSet();
@@ -911,7 +924,7 @@ void collision_detection::CollisionData::enableGroup(const robot_model::RobotMod
active_components_only_ = nullptr;
}
-void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd* manager)
+void FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd* manager)
{
std::vector collision_objects(collision_objects_.size());
for (std::size_t i = 0; i < collision_objects_.size(); ++i)
@@ -920,14 +933,15 @@ void collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManagerd
manager->registerObjects(collision_objects);
}
-void collision_detection::FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager)
+void FCLObject::unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager)
{
for (auto& collision_object : collision_objects_)
manager->unregisterObject(collision_object.get());
}
-void collision_detection::FCLObject::clear()
+void FCLObject::clear()
{
collision_objects_.clear();
collision_geometry_.clear();
}
+} // namespace collision_detection
diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
new file mode 100644
index 0000000000..20ef171fbc
--- /dev/null
+++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
@@ -0,0 +1,434 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Ioan Sucan, Jens Petit */
+
+#include
+#include
+#include
+
+#include
+
+#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
+#include
+#endif
+
+namespace collision_detection
+{
+const std::string CollisionDetectorAllocatorFCL::NAME("FCL");
+
+CollisionEnvFCL::CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, double padding, double scale)
+ : CollisionEnv(model, padding, scale)
+{
+ const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry();
+ std::size_t index;
+ robot_geoms_.resize(robot_model_->getLinkGeometryCount());
+ robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
+ // we keep the same order of objects as what RobotState *::getLinkState() returns
+ for (auto link : links)
+ for (std::size_t j = 0; j < link->getShapes().size(); ++j)
+ {
+ FCLGeometryConstPtr link_geometry = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
+ getLinkPadding(link->getName()), link, j);
+ if (link_geometry)
+ {
+ index = link->getFirstCollisionBodyTransformIndex() + j;
+ robot_geoms_[index] = link_geometry;
+
+ // Need to store the FCL object so the AABB does not get recreated every time.
+ // Every time this object is created, g->computeLocalAABB() is called which is
+ // very expensive and should only be calculated once. To update the AABB, use the
+ // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
+ robot_fcl_objs_[index] =
+ FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(link_geometry->collision_geometry_));
+ }
+ else
+ ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'",
+ link->getName().c_str());
+ }
+
+ auto m = new fcl::DynamicAABBTreeCollisionManagerd();
+ // m->tree_init_level = 2;
+ manager_.reset(m);
+
+ // request notifications about changes to new world
+ observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
+}
+
+CollisionEnvFCL::CollisionEnvFCL(const robot_model::RobotModelConstPtr& model, const WorldPtr& world, double padding,
+ double scale)
+ : CollisionEnv(model, world, padding, scale)
+{
+ const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry();
+ std::size_t index;
+ robot_geoms_.resize(robot_model_->getLinkGeometryCount());
+ robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
+ // we keep the same order of objects as what RobotState *::getLinkState() returns
+ for (auto link : links)
+ for (std::size_t j = 0; j < link->getShapes().size(); ++j)
+ {
+ FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
+ getLinkPadding(link->getName()), link, j);
+ if (g)
+ {
+ index = link->getFirstCollisionBodyTransformIndex() + j;
+ robot_geoms_[index] = g;
+
+ // Need to store the FCL object so the AABB does not get recreated every time.
+ // Every time this object is created, g->computeLocalAABB() is called which is
+ // very expensive and should only be calculated once. To update the AABB, use the
+ // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
+ robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_));
+ }
+ else
+ ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'",
+ link->getName().c_str());
+ }
+
+ auto m = new fcl::DynamicAABBTreeCollisionManagerd();
+ // m->tree_init_level = 2;
+ manager_.reset(m);
+
+ // request notifications about changes to new world
+ observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
+ getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
+}
+
+CollisionEnvFCL::~CollisionEnvFCL()
+{
+ getWorld()->removeObserver(observer_handle_);
+}
+
+CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world) : CollisionEnv(other, world)
+{
+ robot_geoms_ = other.robot_geoms_;
+ robot_fcl_objs_ = other.robot_fcl_objs_;
+
+ auto m = new fcl::DynamicAABBTreeCollisionManagerd();
+ // m->tree_init_level = 2;
+ manager_.reset(m);
+
+ fcl_objs_ = other.fcl_objs_;
+ for (auto& fcl_obj : fcl_objs_)
+ fcl_obj.second.registerTo(manager_.get());
+ // manager_->update();
+
+ // request notifications about changes to new world
+ observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
+}
+
+void CollisionEnvFCL::getAttachedBodyObjects(const robot_state::AttachedBody* ab,
+ std::vector& geoms) const
+{
+ const std::vector& shapes = ab->getShapes();
+ for (std::size_t i = 0; i < shapes.size(); ++i)
+ {
+ FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab, i);
+ if (co)
+ geoms.push_back(co);
+ }
+}
+
+void CollisionEnvFCL::constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const
+{
+ for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
+ {
+ FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
+ if (g)
+ {
+ auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->shape_poses_[i]));
+ fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co));
+ fcl_obj.collision_geometry_.push_back(g);
+ }
+ }
+}
+
+void CollisionEnvFCL::constructFCLObjectRobot(const robot_state::RobotState& state, FCLObject& fcl_obj) const
+{
+ fcl_obj.collision_objects_.reserve(robot_geoms_.size());
+ fcl::Transform3d fcl_tf;
+
+ for (std::size_t i = 0; i < robot_geoms_.size(); ++i)
+ if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_)
+ {
+ transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link,
+ robot_geoms_[i]->collision_geometry_data_->shape_index),
+ fcl_tf);
+ auto coll_obj = new fcl::CollisionObjectd(*robot_fcl_objs_[i]);
+ coll_obj->setTransform(fcl_tf);
+ coll_obj->computeAABB();
+ fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj));
+ }
+
+ // TODO: Implement a method for caching fcl::CollisionObject's for robot_state::AttachedBody's
+ std::vector ab;
+ state.getAttachedBodies(ab);
+ for (auto& body : ab)
+ {
+ std::vector objs;
+ getAttachedBodyObjects(body, objs);
+ const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms();
+ for (std::size_t k = 0; k < objs.size(); ++k)
+ if (objs[k]->collision_geometry_)
+ {
+ transform2fcl(ab_t[k], fcl_tf);
+ fcl_obj.collision_objects_.push_back(
+ FCLCollisionObjectPtr(new fcl::CollisionObjectd(objs[k]->collision_geometry_, fcl_tf)));
+ // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
+ // and would be destroyed when objs goes out of scope.
+ fcl_obj.collision_geometry_.push_back(objs[k]);
+ }
+ }
+}
+
+void CollisionEnvFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const
+{
+ auto m = new fcl::DynamicAABBTreeCollisionManagerd();
+ // m->tree_init_level = 2;
+ manager.manager_.reset(m);
+ constructFCLObjectRobot(state, manager.object_);
+ manager.object_.registerTo(manager.manager_.get());
+ // manager.manager_->update();
+}
+
+void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const
+{
+ checkSelfCollisionHelper(req, res, state, nullptr);
+}
+
+void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const
+{
+ checkSelfCollisionHelper(req, res, state, &acm);
+}
+
+void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state,
+ const AllowedCollisionMatrix* acm) const
+{
+ FCLManager manager;
+ allocSelfCollisionBroadPhase(state, manager);
+ CollisionData cd(&req, &res, acm);
+ cd.enableGroup(getRobotModel());
+ manager.manager_->collide(&cd, &collisionCallback);
+ if (req.distance)
+ {
+ DistanceRequest dreq;
+ DistanceResult dres;
+
+ dreq.group_name = req.group_name;
+ dreq.acm = acm;
+ dreq.enableGroup(getRobotModel());
+ distanceSelf(dreq, dres, state);
+ res.distance = dres.minimum_distance.distance;
+ }
+}
+
+void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state) const
+{
+ checkRobotCollisionHelper(req, res, state, nullptr);
+}
+
+void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const
+{
+ checkRobotCollisionHelper(req, res, state, &acm);
+}
+
+void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state1,
+ const robot_state::RobotState& state2) const
+{
+ ROS_ERROR_NAMED("collision_detection.bullet", "Continuous collision not implemented");
+}
+
+void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state1, const robot_state::RobotState& state2,
+ const AllowedCollisionMatrix& acm) const
+{
+ ROS_ERROR_NAMED("collision_detection.fcl", "Not implemented");
+}
+
+void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res,
+ const robot_state::RobotState& state,
+ const AllowedCollisionMatrix* acm) const
+{
+ FCLObject fcl_obj;
+ constructFCLObjectRobot(state, fcl_obj);
+
+ CollisionData cd(&req, &res, acm);
+ cd.enableGroup(getRobotModel());
+ for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
+ manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
+
+ if (req.distance)
+ {
+ DistanceRequest dreq;
+ DistanceResult dres;
+
+ dreq.group_name = req.group_name;
+ dreq.acm = acm;
+ dreq.enableGroup(getRobotModel());
+ distanceRobot(dreq, dres, state);
+ res.distance = dres.minimum_distance.distance;
+ }
+}
+
+void CollisionEnvFCL::distanceSelf(const DistanceRequest& req, DistanceResult& res,
+ const robot_state::RobotState& state) const
+{
+ FCLManager manager;
+ allocSelfCollisionBroadPhase(state, manager);
+ DistanceData drd(&req, &res);
+
+ manager.manager_->distance(&drd, &distanceCallback);
+}
+
+void CollisionEnvFCL::distanceRobot(const DistanceRequest& req, DistanceResult& res,
+ const robot_state::RobotState& state) const
+{
+ FCLObject fcl_obj;
+ constructFCLObjectRobot(state, fcl_obj);
+
+ DistanceData drd(&req, &res);
+ for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i)
+ manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback);
+}
+
+void CollisionEnvFCL::updateFCLObject(const std::string& id)
+{
+ // remove FCL objects that correspond to this object
+ auto jt = fcl_objs_.find(id);
+ if (jt != fcl_objs_.end())
+ {
+ jt->second.unregisterFrom(manager_.get());
+ jt->second.clear();
+ }
+
+ // check to see if we have this object
+ auto it = getWorld()->find(id);
+ if (it != getWorld()->end())
+ {
+ // construct FCL objects that correspond to this object
+ if (jt != fcl_objs_.end())
+ {
+ constructFCLObjectWorld(it->second.get(), jt->second);
+ jt->second.registerTo(manager_.get());
+ }
+ else
+ {
+ constructFCLObjectWorld(it->second.get(), fcl_objs_[id]);
+ fcl_objs_[id].registerTo(manager_.get());
+ }
+ }
+ else
+ {
+ if (jt != fcl_objs_.end())
+ fcl_objs_.erase(jt);
+ }
+
+ // manager_->update();
+}
+
+void CollisionEnvFCL::setWorld(const WorldPtr& world)
+{
+ if (world == getWorld())
+ return;
+
+ // turn off notifications about old world
+ getWorld()->removeObserver(observer_handle_);
+
+ // clear out objects from old world
+ manager_->clear();
+ fcl_objs_.clear();
+ cleanCollisionGeometryCache();
+
+ CollisionEnv::setWorld(world);
+
+ // request notifications about changes to new world
+ observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionEnvFCL::notifyObjectChange, this, _1, _2));
+
+ // get notifications any objects already in the new world
+ getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
+}
+
+void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action)
+{
+ if (action == World::DESTROY)
+ {
+ auto it = fcl_objs_.find(obj->id_);
+ if (it != fcl_objs_.end())
+ {
+ it->second.unregisterFrom(manager_.get());
+ it->second.clear();
+ fcl_objs_.erase(it);
+ }
+ cleanCollisionGeometryCache();
+ }
+ else
+ {
+ updateFCLObject(obj->id_);
+ if (action & (World::DESTROY | World::REMOVE_SHAPE))
+ cleanCollisionGeometryCache();
+ }
+}
+
+void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector& links)
+{
+ std::size_t index;
+ for (const auto& link : links)
+ {
+ const robot_model::LinkModel* lmodel = robot_model_->getLinkModel(link);
+ if (lmodel)
+ {
+ for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j)
+ {
+ FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()),
+ getLinkPadding(lmodel->getName()), lmodel, j);
+ if (g)
+ {
+ index = lmodel->getFirstCollisionBodyTransformIndex() + j;
+ robot_geoms_[index] = g;
+ robot_fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_));
+ }
+ }
+ }
+ else
+ ROS_ERROR_NAMED("collision_detection.fcl", "Updating padding or scaling for unknown link: '%s'", link.c_str());
+ }
+}
+
+} // end of namespace collision_detection
diff --git a/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp
deleted file mode 100644
index 9b8a4cea68..0000000000
--- a/moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp
+++ /dev/null
@@ -1,307 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Ioan Sucan */
-
-#include
-#include
-
-#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
-#include
-#endif
-
-namespace collision_detection
-{
-CollisionRobotFCL::CollisionRobotFCL(const robot_model::RobotModelConstPtr& model, double padding, double scale)
- : CollisionRobot(model, padding, scale)
-{
- const std::vector& links = robot_model_->getLinkModelsWithCollisionGeometry();
- std::size_t index;
- geoms_.resize(robot_model_->getLinkGeometryCount());
- fcl_objs_.resize(robot_model_->getLinkGeometryCount());
- // we keep the same order of objects as what RobotState *::getLinkState() returns
- for (auto link : links)
- for (std::size_t j = 0; j < link->getShapes().size(); ++j)
- {
- FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
- getLinkPadding(link->getName()), link, j);
- if (g)
- {
- index = link->getFirstCollisionBodyTransformIndex() + j;
- geoms_[index] = g;
-
- // Need to store the FCL object so the AABB does not get recreated every time.
- // Every time this object is created, g->computeLocalAABB() is called which is
- // very expensive and should only be calculated once. To update the AABB, use the
- // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
- fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_));
- }
- else
- ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'",
- link->getName().c_str());
- }
-}
-
-CollisionRobotFCL::CollisionRobotFCL(const CollisionRobotFCL& other) : CollisionRobot(other)
-{
- geoms_ = other.geoms_;
- fcl_objs_ = other.fcl_objs_;
-}
-
-void CollisionRobotFCL::getAttachedBodyObjects(const robot_state::AttachedBody* ab,
- std::vector& geoms) const
-{
- const std::vector& shapes = ab->getShapes();
- for (std::size_t i = 0; i < shapes.size(); ++i)
- {
- FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], ab, i);
- if (co)
- geoms.push_back(co);
- }
-}
-
-void CollisionRobotFCL::constructFCLObject(const robot_state::RobotState& state, FCLObject& fcl_obj) const
-{
- fcl_obj.collision_objects_.reserve(geoms_.size());
- fcl::Transform3d fcl_tf;
-
- for (std::size_t i = 0; i < geoms_.size(); ++i)
- if (geoms_[i] && geoms_[i]->collision_geometry_)
- {
- transform2fcl(state.getCollisionBodyTransform(geoms_[i]->collision_geometry_data_->ptr.link,
- geoms_[i]->collision_geometry_data_->shape_index),
- fcl_tf);
- auto coll_obj = new fcl::CollisionObjectd(*fcl_objs_[i]);
- coll_obj->setTransform(fcl_tf);
- coll_obj->computeAABB();
- fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj));
- }
-
- // TODO: Implement a method for caching fcl::CollisionObject's for robot_state::AttachedBody's
- std::vector ab;
- state.getAttachedBodies(ab);
- for (auto& body : ab)
- {
- std::vector objs;
- getAttachedBodyObjects(body, objs);
- const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms();
- for (std::size_t k = 0; k < objs.size(); ++k)
- if (objs[k]->collision_geometry_)
- {
- transform2fcl(ab_t[k], fcl_tf);
- fcl_obj.collision_objects_.push_back(
- FCLCollisionObjectPtr(new fcl::CollisionObjectd(objs[k]->collision_geometry_, fcl_tf)));
- // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
- // and would be destroyed when objs goes out of scope.
- fcl_obj.collision_geometry_.push_back(objs[k]);
- }
- }
-}
-
-void CollisionRobotFCL::allocSelfCollisionBroadPhase(const robot_state::RobotState& state, FCLManager& manager) const
-{
- auto m = new fcl::DynamicAABBTreeCollisionManagerd();
- // m->tree_init_level = 2;
- manager.manager_.reset(m);
- constructFCLObject(state, manager.object_);
- manager.object_.registerTo(manager.manager_.get());
- // manager.manager_->update();
-}
-
-void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state) const
-{
- checkSelfCollisionHelper(req, res, state, nullptr);
-}
-
-void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const
-{
- checkSelfCollisionHelper(req, res, state, &acm);
-}
-
-void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2) const
-{
- ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
-}
-
-void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1, const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const
-{
- ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
-}
-
-void CollisionRobotFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state,
- const AllowedCollisionMatrix* acm) const
-{
- FCLManager manager;
- allocSelfCollisionBroadPhase(state, manager);
- CollisionData cd(&req, &res, acm);
- cd.enableGroup(getRobotModel());
- manager.manager_->collide(&cd, &collisionCallback);
- if (req.distance)
- {
- DistanceRequest dreq;
- DistanceResult dres;
-
- dreq.group_name = req.group_name;
- dreq.acm = acm;
- dreq.enableGroup(getRobotModel());
- distanceSelf(dreq, dres, state);
- res.distance = dres.minimum_distance.distance;
- }
-}
-
-void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const
-{
- checkOtherCollisionHelper(req, res, state, other_robot, other_state, nullptr);
-}
-
-void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state,
- const AllowedCollisionMatrix& acm) const
-{
- checkOtherCollisionHelper(req, res, state, other_robot, other_state, &acm);
-}
-
-void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state1,
- const robot_state::RobotState& other_state2) const
-{
- ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
-}
-
-void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state1,
- const robot_state::RobotState& state2, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state1,
- const robot_state::RobotState& other_state2,
- const AllowedCollisionMatrix& acm) const
-{
- ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
-}
-
-void CollisionRobotFCL::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res,
- const robot_state::RobotState& state,
- const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state,
- const AllowedCollisionMatrix* acm) const
-{
- FCLManager manager;
- allocSelfCollisionBroadPhase(state, manager);
-
- const CollisionRobotFCL& fcl_rob = dynamic_cast(other_robot);
- FCLObject other_fcl_obj;
- fcl_rob.constructFCLObject(other_state, other_fcl_obj);
-
- CollisionData cd(&req, &res, acm);
- cd.enableGroup(getRobotModel());
- for (std::size_t i = 0; !cd.done_ && i < other_fcl_obj.collision_objects_.size(); ++i)
- manager.manager_->collide(other_fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
-
- if (req.distance)
- {
- DistanceRequest dreq;
- DistanceResult dres;
-
- dreq.group_name = req.group_name;
- dreq.acm = acm;
- dreq.enableGroup(getRobotModel());
- distanceOther(dreq, dres, state, other_robot, other_state);
- res.distance = dres.minimum_distance.distance;
- }
-}
-
-void CollisionRobotFCL::updatedPaddingOrScaling(const std::vector& links)
-{
- std::size_t index;
- for (const auto& link : links)
- {
- const robot_model::LinkModel* lmodel = robot_model_->getLinkModel(link);
- if (lmodel)
- {
- for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j)
- {
- FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()),
- getLinkPadding(lmodel->getName()), lmodel, j);
- if (g)
- {
- index = lmodel->getFirstCollisionBodyTransformIndex() + j;
- geoms_[index] = g;
- fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_));
- }
- }
- }
- else
- ROS_ERROR_NAMED("collision_detection.fcl", "Updating padding or scaling for unknown link: '%s'", link.c_str());
- }
-}
-
-void CollisionRobotFCL::distanceSelf(const DistanceRequest& req, DistanceResult& res,
- const robot_state::RobotState& state) const
-{
- FCLManager manager;
- allocSelfCollisionBroadPhase(state, manager);
- DistanceData drd(&req, &res);
-
- manager.manager_->distance(&drd, &distanceCallback);
-}
-
-void CollisionRobotFCL::distanceOther(const DistanceRequest& req, DistanceResult& res,
- const robot_state::RobotState& state, const CollisionRobot& other_robot,
- const robot_state::RobotState& other_state) const
-{
- FCLManager manager;
- allocSelfCollisionBroadPhase(state, manager);
-
- const CollisionRobotFCL& fcl_rob = dynamic_cast(other_robot);
- FCLObject other_fcl_obj;
- fcl_rob.constructFCLObject(other_state, other_fcl_obj);
-
- DistanceData drd(&req, &res);
- for (std::size_t i = 0; !drd.done && i < other_fcl_obj.collision_objects_.size(); ++i)
- manager.manager_->distance(other_fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback);
-}
-
-} // end of namespace collision_detection
diff --git a/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp
deleted file mode 100644
index 8d6eb392f9..0000000000
--- a/moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp
+++ /dev/null
@@ -1,299 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/* Author: Ioan Sucan */
-
-#include
-#include
-#include
-
-#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
-#include
-#include
-#include
-#include
-#else
-#include
-#include
-#include
-#include
-#endif
-
-#include
-
-namespace collision_detection
-{
-const std::string CollisionDetectorAllocatorFCL::NAME("FCL");
-
-CollisionWorldFCL::CollisionWorldFCL() : CollisionWorld()
-{
- auto m = new fcl::DynamicAABBTreeCollisionManagerd();
- // m->tree_init_level = 2;
- manager_.reset(m);
-
- // request notifications about changes to new world
- observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
-}
-
-CollisionWorldFCL::CollisionWorldFCL(const WorldPtr& world) : CollisionWorld(world)
-{
- auto m = new fcl::DynamicAABBTreeCollisionManagerd();
- // m->tree_init_level = 2;
- manager_.reset(m);
-
- // request notifications about changes to new world
- observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
- getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
-}
-
-CollisionWorldFCL::CollisionWorldFCL(const CollisionWorldFCL& other, const WorldPtr& world)
- : CollisionWorld(other, world)
-{
- auto m = new fcl::DynamicAABBTreeCollisionManagerd();
- // m->tree_init_level = 2;
- manager_.reset(m);
-
- fcl_objs_ = other.fcl_objs_;
- for (auto& fcl_obj : fcl_objs_)
- fcl_obj.second.registerTo(manager_.get());
- // manager_->update();
-
- // request notifications about changes to new world
- observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
-}
-
-CollisionWorldFCL::~CollisionWorldFCL()
-{
- getWorld()->removeObserver(observer_handle_);
-}
-
-void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot, const robot_state::RobotState& state) const
-{
- checkRobotCollisionHelper(req, res, robot, state, nullptr);
-}
-
-void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot, const robot_state::RobotState& state,
- const AllowedCollisionMatrix& acm) const
-{
- checkRobotCollisionHelper(req, res, robot, state, &acm);
-}
-
-void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2) const
-{
- ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
-}
-
-void CollisionWorldFCL::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot, const robot_state::RobotState& state1,
- const robot_state::RobotState& state2,
- const AllowedCollisionMatrix& acm) const
-{
- ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
-}
-
-void CollisionWorldFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res,
- const CollisionRobot& robot, const robot_state::RobotState& state,
- const AllowedCollisionMatrix* acm) const
-{
- const CollisionRobotFCL& robot_fcl = dynamic_cast(robot);
- FCLObject fcl_obj;
- robot_fcl.constructFCLObject(state, fcl_obj);
-
- CollisionData cd(&req, &res, acm);
- cd.enableGroup(robot.getRobotModel());
- for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
- manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
-
- if (req.distance)
- {
- DistanceRequest dreq;
- DistanceResult dres;
-
- dreq.group_name = req.group_name;
- dreq.acm = acm;
- dreq.enableGroup(robot.getRobotModel());
- distanceRobot(dreq, dres, robot, state);
- res.distance = dres.minimum_distance.distance;
- }
-}
-
-void CollisionWorldFCL::checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionWorld& other_world) const
-{
- checkWorldCollisionHelper(req, res, other_world, nullptr);
-}
-
-void CollisionWorldFCL::checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
- const CollisionWorld& other_world, const AllowedCollisionMatrix& acm) const
-{
- checkWorldCollisionHelper(req, res, other_world, &acm);
-}
-
-void CollisionWorldFCL::checkWorldCollisionHelper(const CollisionRequest& req, CollisionResult& res,
- const CollisionWorld& other_world,
- const AllowedCollisionMatrix* acm) const
-{
- const CollisionWorldFCL& other_fcl_world = dynamic_cast(other_world);
- CollisionData cd(&req, &res, acm);
- manager_->collide(other_fcl_world.manager_.get(), &cd, &collisionCallback);
-
- if (req.distance)
- {
- DistanceRequest dreq;
- DistanceResult dres;
-
- dreq.group_name = req.group_name;
- dreq.acm = acm;
- distanceWorld(dreq, dres, other_world);
- res.distance = dres.minimum_distance.distance;
- }
-}
-
-void CollisionWorldFCL::constructFCLObject(const World::Object* obj, FCLObject& fcl_obj) const
-{
- for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
- {
- FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
- if (g)
- {
- auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->shape_poses_[i]));
- fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co));
- fcl_obj.collision_geometry_.push_back(g);
- }
- }
-}
-
-void CollisionWorldFCL::updateFCLObject(const std::string& id)
-{
- // remove FCL objects that correspond to this object
- auto jt = fcl_objs_.find(id);
- if (jt != fcl_objs_.end())
- {
- jt->second.unregisterFrom(manager_.get());
- jt->second.clear();
- }
-
- // check to see if we have this object
- auto it = getWorld()->find(id);
- if (it != getWorld()->end())
- {
- // construct FCL objects that correspond to this object
- if (jt != fcl_objs_.end())
- {
- constructFCLObject(it->second.get(), jt->second);
- jt->second.registerTo(manager_.get());
- }
- else
- {
- constructFCLObject(it->second.get(), fcl_objs_[id]);
- fcl_objs_[id].registerTo(manager_.get());
- }
- }
- else
- {
- if (jt != fcl_objs_.end())
- fcl_objs_.erase(jt);
- }
-
- // manager_->update();
-}
-
-void CollisionWorldFCL::setWorld(const WorldPtr& world)
-{
- if (world == getWorld())
- return;
-
- // turn off notifications about old world
- getWorld()->removeObserver(observer_handle_);
-
- // clear out objects from old world
- manager_->clear();
- fcl_objs_.clear();
- cleanCollisionGeometryCache();
-
- CollisionWorld::setWorld(world);
-
- // request notifications about changes to new world
- observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
-
- // get notifications any objects already in the new world
- getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
-}
-
-void CollisionWorldFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action)
-{
- if (action == World::DESTROY)
- {
- auto it = fcl_objs_.find(obj->id_);
- if (it != fcl_objs_.end())
- {
- it->second.unregisterFrom(manager_.get());
- it->second.clear();
- fcl_objs_.erase(it);
- }
- cleanCollisionGeometryCache();
- }
- else
- {
- updateFCLObject(obj->id_);
- if (action & (World::DESTROY | World::REMOVE_SHAPE))
- cleanCollisionGeometryCache();
- }
-}
-
-void CollisionWorldFCL::distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot,
- const robot_state::RobotState& state) const
-{
- const CollisionRobotFCL& robot_fcl = dynamic_cast(robot);
- FCLObject fcl_obj;
- robot_fcl.constructFCLObject(state, fcl_obj);
-
- DistanceData drd(&req, &res);
- for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i)
- manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback);
-}
-
-void CollisionWorldFCL::distanceWorld(const DistanceRequest& req, DistanceResult& res,
- const CollisionWorld& world) const
-{
- const CollisionWorldFCL& other_fcl_world = dynamic_cast(world);
- DistanceData drd(&req, &res);
- manager_->distance(other_fcl_world.manager_.get(), &drd, &distanceCallback);
-}
-
-} // end of namespace collision_detection
diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp
index e606f632a3..e4ea360a95 100644
--- a/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp
+++ b/moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp
@@ -36,8 +36,7 @@
#include
#include
-#include
-#include
+#include
#include
#include
@@ -49,8 +48,7 @@
#include
#include
-typedef collision_detection::CollisionWorldFCL DefaultCWorldType;
-typedef collision_detection::CollisionRobotFCL DefaultCRobotType;
+typedef collision_detection::CollisionEnvFCL DefaultEnvType;
class FclCollisionDetectionTester : public testing::Test
{
@@ -63,8 +61,7 @@ class FclCollisionDetectionTester : public testing::Test
acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
- crobot_.reset(new DefaultCRobotType(robot_model_));
- cworld_.reset(new DefaultCWorldType());
+ c_env_.reset(new DefaultEnvType(robot_model_));
}
void TearDown() override
@@ -76,8 +73,7 @@ class FclCollisionDetectionTester : public testing::Test
robot_model::RobotModelPtr robot_model_;
- collision_detection::CollisionRobotPtr crobot_;
- collision_detection::CollisionWorldPtr cworld_;
+ collision_detection::CollisionEnvPtr c_env_;
collision_detection::AllowedCollisionMatrixPtr acm_;
@@ -97,7 +93,7 @@ TEST_F(FclCollisionDetectionTester, DefaultNotInCollision)
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
- crobot_->checkSelfCollision(req, res, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_FALSE(res.collision);
}
@@ -124,11 +120,11 @@ TEST_F(FclCollisionDetectionTester, LinksInCollision)
robot_state.update();
acm_->setEntry("base_link", "base_bellow_link", false);
- crobot_->checkSelfCollision(req, res1, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res1, robot_state, *acm_);
ASSERT_TRUE(res1.collision);
acm_->setEntry("base_link", "base_bellow_link", true);
- crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res2, robot_state, *acm_);
ASSERT_FALSE(res2.collision);
// req.verbose = true;
@@ -139,7 +135,7 @@ TEST_F(FclCollisionDetectionTester, LinksInCollision)
robot_state.update();
acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
- crobot_->checkSelfCollision(req, res3, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res3, robot_state, *acm_);
ASSERT_TRUE(res3.collision);
}
@@ -171,7 +167,7 @@ TEST_F(FclCollisionDetectionTester, ContactReporting)
acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
collision_detection::CollisionResult res;
- crobot_->checkSelfCollision(req, res, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_TRUE(res.collision);
EXPECT_EQ(res.contacts.size(), 1u);
EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
@@ -180,7 +176,7 @@ TEST_F(FclCollisionDetectionTester, ContactReporting)
req.max_contacts = 2;
req.max_contacts_per_pair = 1;
// req.verbose = true;
- crobot_->checkSelfCollision(req, res, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_TRUE(res.collision);
EXPECT_EQ(res.contacts.size(), 2u);
EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
@@ -191,7 +187,7 @@ TEST_F(FclCollisionDetectionTester, ContactReporting)
req.max_contacts = 10;
req.max_contacts_per_pair = 2;
acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false));
- crobot_->checkSelfCollision(req, res, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_TRUE(res.collision);
EXPECT_LE(res.contacts.size(), 10u);
EXPECT_LE(res.contact_count, 10u);
@@ -222,7 +218,7 @@ TEST_F(FclCollisionDetectionTester, ContactPositions)
acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
collision_detection::CollisionResult res;
- crobot_->checkSelfCollision(req, res, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_TRUE(res.collision);
ASSERT_EQ(res.contacts.size(), 1u);
ASSERT_EQ(res.contacts.begin()->second.size(), 1u);
@@ -242,7 +238,7 @@ TEST_F(FclCollisionDetectionTester, ContactPositions)
robot_state.update();
collision_detection::CollisionResult res2;
- crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res2, robot_state, *acm_);
ASSERT_TRUE(res2.collision);
ASSERT_EQ(res2.contacts.size(), 1u);
ASSERT_EQ(res2.contacts.begin()->second.size(), 1u);
@@ -262,7 +258,7 @@ TEST_F(FclCollisionDetectionTester, ContactPositions)
robot_state.update();
collision_detection::CollisionResult res3;
- crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res2, robot_state, *acm_);
ASSERT_FALSE(res3.collision);
}
@@ -283,18 +279,18 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester)
// robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
robot_state.update();
- crobot_->checkSelfCollision(req, res, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_FALSE(res.collision);
shapes::Shape* shape = new shapes::Box(.1, .1, .1);
- cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
+ c_env_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
res = collision_detection::CollisionResult();
- cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
+ c_env_->checkRobotCollision(req, res, robot_state, *acm_);
ASSERT_TRUE(res.collision);
// deletes shape
- cworld_->getWorld()->removeObject("box");
+ c_env_->getWorld()->removeObject("box");
shape = new shapes::Box(.1, .1, .1);
std::vector shapes;
@@ -305,7 +301,7 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester)
robot_state.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
res = collision_detection::CollisionResult();
- crobot_->checkSelfCollision(req, res, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_TRUE(res.collision);
// deletes shape
@@ -318,19 +314,19 @@ TEST_F(FclCollisionDetectionTester, AttachedBodyTester)
robot_state.update();
res = collision_detection::CollisionResult();
- crobot_->checkSelfCollision(req, res, robot_state, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state, *acm_);
ASSERT_FALSE(res.collision);
pos1.translation().x() = 5.01;
shapes::Shape* coll = new shapes::Box(.1, .1, .1);
- cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
+ c_env_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
res = collision_detection::CollisionResult();
- cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
+ c_env_->checkRobotCollision(req, res, robot_state, *acm_);
ASSERT_TRUE(res.collision);
acm_->setEntry("coll", "r_gripper_palm_link", true);
res = collision_detection::CollisionResult();
- cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
+ c_env_->checkRobotCollision(req, res, robot_state, *acm_);
ASSERT_TRUE(res.collision);
}
@@ -342,15 +338,15 @@ TEST_F(FclCollisionDetectionTester, DiffSceneTester)
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
-
- collision_detection::CollisionRobotFCL new_crobot(
- *(dynamic_cast(crobot_.get())));
+ collision_detection::CollisionEnvFCL* casted_env_pointer =
+ dynamic_cast(c_env_.get());
+ collision_detection::CollisionEnvFCL new_c_env(*casted_env_pointer, c_env_->getWorld());
ros::WallTime before = ros::WallTime::now();
- new_crobot.checkSelfCollision(req, res, robot_state);
+ new_c_env.checkSelfCollision(req, res, robot_state);
double first_check = (ros::WallTime::now() - before).toSec();
before = ros::WallTime::now();
- new_crobot.checkSelfCollision(req, res, robot_state);
+ new_c_env.checkSelfCollision(req, res, robot_state);
double second_check = (ros::WallTime::now() - before).toSec();
EXPECT_LT(fabs(first_check - second_check), .05);
@@ -367,22 +363,21 @@ TEST_F(FclCollisionDetectionTester, DiffSceneTester)
robot_state.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link");
before = ros::WallTime::now();
- new_crobot.checkSelfCollision(req, res, robot_state);
+ new_c_env.checkSelfCollision(req, res, robot_state);
first_check = (ros::WallTime::now() - before).toSec();
before = ros::WallTime::now();
- new_crobot.checkSelfCollision(req, res, robot_state);
+ new_c_env.checkSelfCollision(req, res, robot_state);
second_check = (ros::WallTime::now() - before).toSec();
// the first check is going to take a while, as data must be constructed
EXPECT_LT(second_check, .1);
- collision_detection::CollisionRobotFCL other_new_crobot(
- *(dynamic_cast(crobot_.get())));
+ collision_detection::CollisionEnvFCL other_new_c_env(*casted_env_pointer, c_env_->getWorld());
before = ros::WallTime::now();
- new_crobot.checkSelfCollision(req, res, robot_state);
+ new_c_env.checkSelfCollision(req, res, robot_state);
first_check = (ros::WallTime::now() - before).toSec();
before = ros::WallTime::now();
- new_crobot.checkSelfCollision(req, res, robot_state);
+ new_c_env.checkSelfCollision(req, res, robot_state);
second_check = (ros::WallTime::now() - before).toSec();
EXPECT_LT(fabs(first_check - second_check), .05);
@@ -398,23 +393,23 @@ TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached)
Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
pos2.translation().x() = 10.0;
- cworld_->getWorld()->addToObject("kinect", shape, pos1);
+ c_env_->getWorld()->addToObject("kinect", shape, pos1);
robot_state::RobotState robot_state(robot_model_);
robot_state.setToDefaultValues();
robot_state.update();
ros::WallTime before = ros::WallTime::now();
- cworld_->checkRobotCollision(req, res, *crobot_, robot_state);
+ c_env_->checkRobotCollision(req, res, robot_state);
double first_check = (ros::WallTime::now() - before).toSec();
before = ros::WallTime::now();
- cworld_->checkRobotCollision(req, res, *crobot_, robot_state);
+ c_env_->checkRobotCollision(req, res, robot_state);
double second_check = (ros::WallTime::now() - before).toSec();
EXPECT_LT(second_check, .05);
- collision_detection::CollisionWorld::ObjectConstPtr object = cworld_->getWorld()->getObject("kinect");
- cworld_->getWorld()->removeObject("kinect");
+ collision_detection::CollisionEnv::ObjectConstPtr object = c_env_->getWorld()->getObject("kinect");
+ c_env_->getWorld()->removeObject("kinect");
robot_state::RobotState robot_state1(robot_model_);
robot_state::RobotState robot_state2(robot_model_);
@@ -434,17 +429,17 @@ TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached)
// going to take a while, but that's fine
res = collision_detection::CollisionResult();
- crobot_->checkSelfCollision(req, res, robot_state1);
+ c_env_->checkSelfCollision(req, res, robot_state1);
EXPECT_TRUE(res.collision);
before = ros::WallTime::now();
- crobot_->checkSelfCollision(req, res, robot_state1, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state1, *acm_);
first_check = (ros::WallTime::now() - before).toSec();
before = ros::WallTime::now();
req.verbose = true;
res = collision_detection::CollisionResult();
- crobot_->checkSelfCollision(req, res, robot_state2, *acm_);
+ c_env_->checkSelfCollision(req, res, robot_state2, *acm_);
second_check = (ros::WallTime::now() - before).toSec();
EXPECT_LT(first_check, .05);
@@ -461,7 +456,7 @@ TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed)
shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
}
ros::WallTime start = ros::WallTime::now();
- cworld_->getWorld()->addToObject("map", shapes, poses);
+ c_env_->getWorld()->addToObject("map", shapes, poses);
double t = (ros::WallTime::now() - start).toSec();
EXPECT_GE(1.0, t);
// this is not really a failure; it is just that slow;
@@ -480,16 +475,16 @@ TEST_F(FclCollisionDetectionTester, MoveMesh)
shapes::ShapePtr kinect_shape;
kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_));
- cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
+ c_env_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
Eigen::Isometry3d np;
for (unsigned int i = 0; i < 5; i++)
{
np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity();
- cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np);
+ c_env_->getWorld()->moveShapeInObject("kinect", kinect_shape, np);
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
- cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_);
+ c_env_->checkCollision(req, res, robot_state1, *acm_);
}
}
@@ -508,37 +503,37 @@ TEST_F(FclCollisionDetectionTester, TestChangingShapeSize)
std::vector shapes;
for (unsigned int i = 0; i < 5; i++)
{
- cworld_->getWorld()->removeObject("shape");
+ c_env_->getWorld()->removeObject("shape");
shapes.clear();
poses.clear();
shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
poses.push_back(Eigen::Isometry3d::Identity());
- cworld_->getWorld()->addToObject("shape", shapes, poses);
+ c_env_->getWorld()->addToObject("shape", shapes, poses);
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
- cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_);
+ c_env_->checkCollision(req, res, robot_state1, *acm_);
ASSERT_TRUE(res.collision);
}
Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity();
shapes::ShapePtr kinect_shape;
kinect_shape.reset(shapes::createMeshFromResource(kinect_dae_resource_));
- cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
+ c_env_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
collision_detection::CollisionRequest req2;
collision_detection::CollisionResult res2;
- cworld_->checkCollision(req2, res2, *crobot_, robot_state1, *acm_);
+ c_env_->checkCollision(req2, res2, robot_state1, *acm_);
ASSERT_TRUE(res2.collision);
for (unsigned int i = 0; i < 5; i++)
{
- cworld_->getWorld()->removeObject("shape");
+ c_env_->getWorld()->removeObject("shape");
shapes.clear();
poses.clear();
shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
poses.push_back(Eigen::Isometry3d::Identity());
- cworld_->getWorld()->addToObject("shape", shapes, poses);
+ c_env_->getWorld()->addToObject("shape", shapes, poses);
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
- cworld_->checkCollision(req, res, *crobot_, robot_state1, *acm_);
+ c_env_->checkCollision(req, res, robot_state1, *acm_);
ASSERT_TRUE(res.collision);
}
}
diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp
new file mode 100644
index 0000000000..64f65b64c3
--- /dev/null
+++ b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp
@@ -0,0 +1,324 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2019, Jens Petit
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the copyright holder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Jens Petit */
+
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+
+/** \brief Brings the panda robot in user defined home position */
+inline void setToHome(robot_state::RobotState& panda_state)
+{
+ panda_state.setToDefaultValues();
+ double joint2 = -0.785;
+ double joint4 = -2.356;
+ double joint6 = 1.571;
+ double joint7 = 0.785;
+ panda_state.setJointPositions("panda_joint2", &joint2);
+ panda_state.setJointPositions("panda_joint4", &joint4);
+ panda_state.setJointPositions("panda_joint6", &joint6);
+ panda_state.setJointPositions("panda_joint7", &joint7);
+ panda_state.update();
+}
+
+class CollisionDetectionEnvTest : public testing::Test
+{
+protected:
+ void SetUp() override
+ {
+ robot_model_ = moveit::core::loadTestingRobotModel("panda");
+ robot_model_ok_ = static_cast(robot_model_);
+
+ acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false));
+
+ acm_->setEntry("panda_link0", "panda_link1", true);
+ acm_->setEntry("panda_link1", "panda_link2", true);
+ acm_->setEntry("panda_link2", "panda_link3", true);
+ acm_->setEntry("panda_link3", "panda_link4", true);
+ acm_->setEntry("panda_link4", "panda_link5", true);
+ acm_->setEntry("panda_link5", "panda_link6", true);
+ acm_->setEntry("panda_link6", "panda_link7", true);
+ acm_->setEntry("panda_link7", "panda_hand", true);
+ acm_->setEntry("panda_hand", "panda_rightfinger", true);
+ acm_->setEntry("panda_hand", "panda_leftfinger", true);
+ acm_->setEntry("panda_rightfinger", "panda_leftfinger", true);
+ acm_->setEntry("panda_link5", "panda_link7", true);
+ acm_->setEntry("panda_link6", "panda_hand", true);
+
+ c_env_.reset(new collision_detection::CollisionEnvFCL(robot_model_));
+
+ robot_state_.reset(new robot_state::RobotState(robot_model_));
+
+ setToHome(*robot_state_);
+ }
+
+ void TearDown() override
+ {
+ }
+
+protected:
+ bool robot_model_ok_;
+
+ robot_model::RobotModelPtr robot_model_;
+
+ collision_detection::CollisionEnvPtr c_env_;
+
+ collision_detection::AllowedCollisionMatrixPtr acm_;
+
+ robot_state::RobotStatePtr robot_state_;
+};
+
+/** \brief Correct setup testing. */
+TEST_F(CollisionDetectionEnvTest, InitOK)
+{
+ ASSERT_TRUE(robot_model_ok_);
+}
+
+/** \brief Tests the default values specified in the SRDF if they are collision free. */
+TEST_F(CollisionDetectionEnvTest, DefaultNotInCollision)
+{
+ collision_detection::CollisionRequest req;
+ collision_detection::CollisionResult res;
+ c_env_->checkSelfCollision(req, res, *robot_state_, *acm_);
+ ASSERT_FALSE(res.collision);
+}
+
+/** \brief A configuration where the robot should collide with itself. */
+TEST_F(CollisionDetectionEnvTest, LinksInCollision)
+{
+ // Sets the joint values to zero which is a colliding configuration
+ robot_state_->setToDefaultValues();
+ robot_state_->update();
+
+ collision_detection::CollisionRequest req;
+ collision_detection::CollisionResult res;
+ c_env_->checkSelfCollision(req, res, *robot_state_, *acm_);
+ ASSERT_TRUE(res.collision);
+}
+
+/** \brief Adding obstacles to the world which are tested against the robot. Simple cases. */
+TEST_F(CollisionDetectionEnvTest, RobotWorldCollision_1)
+{
+ collision_detection::CollisionRequest req;
+ collision_detection::CollisionResult res;
+
+ shapes::Shape* shape = new shapes::Box(.1, .1, .1);
+ shapes::ShapeConstPtr shape_ptr(shape);
+
+ Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
+ pos1.translation().z() = 0.3;
+ c_env_->getWorld()->addToObject("box", shape_ptr, pos1);
+
+ c_env_->checkSelfCollision(req, res, *robot_state_, *acm_);
+ ASSERT_FALSE(res.collision);
+ res.clear();
+
+ c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
+ ASSERT_TRUE(res.collision);
+ res.clear();
+
+ c_env_->getWorld()->moveObject("box", pos1);
+ c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
+ ASSERT_TRUE(res.collision);
+ res.clear();
+
+ c_env_->getWorld()->moveObject("box", pos1);
+ ASSERT_FALSE(res.collision);
+}
+
+/** \brief Adding obstacles to the world which are tested against the robot. */
+TEST_F(CollisionDetectionEnvTest, RobotWorldCollision_2)
+{
+ collision_detection::CollisionRequest req;
+ collision_detection::CollisionResult res;
+ req.max_contacts = 10;
+ req.contacts = true;
+ req.verbose = true;
+
+ shapes::Shape* shape = new shapes::Box(.4, .4, .4);
+ shapes::ShapeConstPtr shape_ptr(shape);
+
+ Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
+ pos1.translation().z() = 0.3;
+ c_env_->getWorld()->addToObject("box", shape_ptr, pos1);
+ c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
+ ASSERT_TRUE(res.collision);
+ ASSERT_GE(res.contact_count, 3u);
+ res.clear();
+}
+
+/** \brief Tests the padding through expanding the link geometry in such a way that a collision occurs. */
+TEST_F(CollisionDetectionEnvTest, PaddingTest)
+{
+ collision_detection::CollisionRequest req;
+ req.contacts = true;
+ req.max_contacts = 10;
+ collision_detection::CollisionResult res;
+
+ c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
+ ASSERT_FALSE(res.collision);
+ res.clear();
+
+ // Adding the box right in front of the robot hand
+ shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1);
+ shapes::ShapeConstPtr shape_ptr(shape);
+
+ Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
+ pos.translation().x() = 0.43;
+ pos.translation().y() = 0;
+ pos.translation().z() = 0.55;
+ c_env_->getWorld()->addToObject("box", shape_ptr, pos);
+
+ c_env_->setLinkPadding("panda_hand", 0.08);
+ c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
+ ASSERT_TRUE(res.collision);
+ res.clear();
+
+ c_env_->setLinkPadding("panda_hand", 0.0);
+ c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
+ ASSERT_FALSE(res.collision);
+}
+
+/** \brief Continuous self collision checks of the robot.
+ *
+ * Functionality not supported yet. */
+TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionSelf)
+{
+ collision_detection::CollisionRequest req;
+ collision_detection::CollisionResult res;
+
+ robot_state::RobotState state1(robot_model_);
+ robot_state::RobotState state2(robot_model_);
+
+ setToHome(state1);
+ double joint2 = 0.15;
+ double joint4 = -3.0;
+ double joint5 = -0.8;
+ double joint7 = -0.785;
+ state1.setJointPositions("panda_joint2", &joint2);
+ state1.setJointPositions("panda_joint4", &joint4);
+ state1.setJointPositions("panda_joint5", &joint5);
+ state1.setJointPositions("panda_joint7", &joint7);
+ state1.update();
+
+ c_env_->checkSelfCollision(req, res, state1, *acm_);
+ ASSERT_FALSE(res.collision);
+ res.clear();
+
+ setToHome(state2);
+ double joint_5_other = 0.8;
+ state2.setJointPositions("panda_joint2", &joint2);
+ state2.setJointPositions("panda_joint4", &joint4);
+ state2.setJointPositions("panda_joint5", &joint_5_other);
+ state2.setJointPositions("panda_joint7", &joint7);
+ state2.update();
+
+ c_env_->checkSelfCollision(req, res, state2, *acm_);
+ ASSERT_FALSE(res.collision);
+ res.clear();
+
+ // c_env_->checkSelfCollision(req, res, state1, state2, *acm_);
+ ROS_INFO_STREAM("Continous to continous collisions are not supported yet, therefore fail here.");
+ ASSERT_TRUE(res.collision);
+ res.clear();
+}
+
+/** \brief Two similar robot poses are used as start and end pose of a continuous collision check.
+ *
+ * Functionality not supported yet. */
+TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionWorld)
+{
+ collision_detection::CollisionRequest req;
+ req.contacts = true;
+ req.max_contacts = 10;
+ collision_detection::CollisionResult res;
+
+ robot_state::RobotState state1(robot_model_);
+ robot_state::RobotState state2(robot_model_);
+
+ setToHome(state1);
+ state1.update();
+
+ setToHome(state2);
+ double joint_2{ 0.05 };
+ double joint_4{ -1.6 };
+ state2.setJointPositions("panda_joint2", &joint_2);
+ state2.setJointPositions("panda_joint4", &joint_4);
+ state2.update();
+
+ c_env_->checkRobotCollision(req, res, state1, state2, *acm_);
+ ASSERT_FALSE(res.collision);
+ res.clear();
+
+ // Adding the box which is not in collision with the individual states but sits right between them.
+ shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1);
+ shapes::ShapeConstPtr shape_ptr(shape);
+
+ Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
+ pos.translation().x() = 0.43;
+ pos.translation().y() = 0;
+ pos.translation().z() = 0.55;
+ c_env_->getWorld()->addToObject("box", shape_ptr, pos);
+
+ c_env_->checkRobotCollision(req, res, state1, *acm_);
+ ASSERT_FALSE(res.collision);
+ res.clear();
+
+ c_env_->checkRobotCollision(req, res, state2, *acm_);
+ ASSERT_FALSE(res.collision);
+ res.clear();
+
+ c_env_->checkRobotCollision(req, res, state1, state2, *acm_);
+ ASSERT_TRUE(res.collision);
+ ASSERT_EQ(res.contact_count, 4u);
+ res.clear();
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt
index 491b47f0e8..063a1331a3 100644
--- a/moveit_core/collision_distance_field/CMakeLists.txt
+++ b/moveit_core/collision_distance_field/CMakeLists.txt
@@ -8,10 +8,8 @@ endif()
add_library(${MOVEIT_LIB_NAME}
src/collision_distance_field_types.cpp
src/collision_common_distance_field.cpp
- src/collision_robot_distance_field.cpp
- src/collision_world_distance_field.cpp
- src/collision_robot_hybrid.cpp
- src/collision_world_hybrid.cpp
+ src/collision_env_distance_field.cpp
+ src/collision_env_hybrid.cpp
)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(${MOVEIT_LIB_NAME}
@@ -30,7 +28,7 @@ endif()
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
- RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h
index fa5910a581..cfb6fe750d 100644
--- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h
+++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h
@@ -34,13 +34,12 @@
/* Author: E. Gil Jones */
-#ifndef MOVEIT_COLLISION_DETECTION_DISTANCE_FIELD_COLLISION_COMMON_
-#define MOVEIT_COLLISION_DETECTION_DISTANCE_FIELD_COLLISION_COMMON_
+#pragma once
#include
#include
#include
-#include
+#include
#include
namespace collision_detection
@@ -180,4 +179,3 @@ PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const rob
void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame,
visualization_msgs::MarkerArray& body_marker_array);
}
-#endif
diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h
index 53c71b9f35..b699170892 100644
--- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h
+++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_detector_allocator_distance_field.h
@@ -34,23 +34,18 @@
/* Author: Acorn Pooley, Ioan Sucan */
-#ifndef MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_DISTANCE_FIELD_H_
-#define MOVEIT_COLLISION_DETECTION_COLLISION_DETECTOR_DISTANCE_FIELD_H_
+#pragma once
#include
-#include
-#include
+#include
namespace collision_detection
{
/** \brief An allocator for Distance Field collision detectors */
class CollisionDetectorAllocatorDistanceField
- : public CollisionDetectorAllocatorTemplate